From: Jochen Sprickerhof Date: Mon, 15 Aug 2016 13:56:43 +0000 (+0200) Subject: Imported Upstream version 1.8.0 X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~13 X-Git-Url: https://dgit.raspbian.org/%22http:/www.example.com/cgi/%22https://%22%22/%22http:/www.example.com/cgi/%22https:/%22%22?a=commitdiff_plain;h=cff328462e82392bce32c3c549a259a22c105ae6;p=pcl.git Imported Upstream version 1.8.0 --- diff --git a/.travis.sh b/.travis.sh index cba0e050..4d7cb299 100755 --- a/.travis.sh +++ b/.travis.sh @@ -10,6 +10,13 @@ ADVANCED_DIR=$BUILD_DIR/doc/advanced/html CMAKE_C_FLAGS="-Wall -Wextra -Wabi -O2" CMAKE_CXX_FLAGS="-Wall -Wextra -Wabi -O2" +DOWNLOAD_DIR=$HOME/download + +export FLANN_ROOT=$HOME/flann +export VTK_DIR=$HOME/vtk +export QHULL_ROOT=$HOME/qhull +export DOXYGEN_DIR=$HOME/doxygen + function build () { case $CC in @@ -69,29 +76,29 @@ function test () { # Configure mkdir $BUILD_DIR && cd $BUILD_DIR - cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS -DPCL_ONLY_CORE_POINT_TYPES=ON -DBUILD_global_tests=ON -DPCL_NO_PRECOMPILE=ON $PCL_DIR + cmake -DCMAKE_C_FLAGS=$CMAKE_C_FLAGS \ + -DCMAKE_CXX_FLAGS=$CMAKE_CXX_FLAGS \ + -DPCL_ONLY_CORE_POINT_TYPES=ON \ + -DBUILD_global_tests=ON \ + -DPCL_NO_PRECOMPILE=ON \ + $PCL_DIR # Build and run tests - make pcl_filters -j3 - make test_filters - make pcl_registration -j3 - make test_registration - make test_registration_api - make tests -j3 + make tests } function doc () { # Do not generate documentation for pull requests if [[ $TRAVIS_PULL_REQUEST != 'false' ]]; then exit; fi - # Install doxygen and sphinx - sudo apt-get install doxygen doxygen-latex graphviz python-pip - sudo pip install sphinx sphinxcontrib-doxylink + # Add installed doxygen to path and install sphinx + export PATH=$DOXYGEN_DIR/bin:$PATH + pip install --user sphinx sphinxcontrib-doxylink # Configure mkdir $BUILD_DIR && cd $BUILD_DIR cmake -DDOXYGEN_USE_SHORT_NAMES=OFF \ -DSPHINX_HTML_FILE_SUFFIX=php \ - -DWITH_DOCS=1 \ - -DWITH_TUTORIALS=1 \ + -DWITH_DOCS=ON \ + -DWITH_TUTORIALS=ON \ $PCL_DIR git config --global user.email "documentation@pointclouds.org" @@ -121,14 +128,205 @@ function doc () # Commit and push cd $DOC_DIR git add --all - git commit --amend -m "Documentation for commit $TRAVIS_COMMIT" + git commit --amend -m "Documentation for commit $TRAVIS_COMMIT" -q git push --force else exit 2 fi } -case $TASK in +function install_flann() +{ + local pkg_ver=1.8.4 + local pkg_file="flann-${pkg_ver}-src" + local pkg_url="http://people.cs.ubc.ca/~mariusm/uploads/FLANN/${pkg_file}.zip" + local pkg_md5sum="a0ecd46be2ee11a68d2a7d9c6b4ce701" + local FLANN_DIR=$HOME/flann + local config=$FLANN_DIR/include/flann/config.h + echo "Installing FLANN ${pkg_ver}" + if [[ -d $FLANN_DIR ]]; then + if [[ -e ${config} ]]; then + local version=`grep -Po "(?<=FLANN_VERSION_ \").*(?=\")" ${config}` + if [[ "$version" = "$pkg_ver" ]]; then + local modified=`stat -c %y ${config} | cut -d ' ' -f1` + echo " > Found cached installation of FLANN" + echo " > Version ${pkg_ver}, built on ${modified}" + return 0 + fi + fi + fi + download ${pkg_url} ${pkg_md5sum} + if [[ $? -ne 0 ]]; then + return $? + fi + unzip -qq pkg + cd ${pkg_file} + mkdir build && cd build + cmake .. \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=$FLANN_DIR \ + -DBUILD_MATLAB_BINDINGS=OFF \ + -DBUILD_PYTHON_BINDINGS=OFF \ + -DBUILD_CUDA_LIB=OFF \ + -DBUILD_C_BINDINGS=OFF \ + -DUSE_OPENMP=OFF + make -j2 && make install && touch ${config} + return $? +} + +function install_vtk() +{ + local pkg_ver=5.10.1 + local pkg_file="vtk-${pkg_ver}" + local pkg_url="http://www.vtk.org/files/release/${pkg_ver:0:4}/${pkg_file}.tar.gz" + local pkg_md5sum="264b0052e65bd6571a84727113508789" + local VTK_DIR=$HOME/vtk + local config=$VTK_DIR/include/vtk-${pkg_ver:0:4}/vtkConfigure.h + echo "Installing VTK ${pkg_ver}" + if [[ -d $VTK_DIR ]]; then + if [[ -e ${config} ]]; then + local version=`grep -Po "(?<=VTK_VERSION \").*(?=\")" ${config}` + if [[ "$version" = "$pkg_ver" ]]; then + local modified=`stat -c %y ${config} | cut -d ' ' -f1` + echo " > Found cached installation of VTK" + echo " > Version ${pkg_ver}, built on ${modified}" + return 0 + fi + fi + fi + download ${pkg_url} ${pkg_md5sum} + if [[ $? -ne 0 ]]; then + return $? + fi + tar xzf pkg + cd "VTK${pkg_ver}" + mkdir build && cd build + cmake .. \ + -Wno-dev \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=ON \ + -DCMAKE_INSTALL_PREFIX=$VTK_DIR \ + -DBUILD_DOCUMENTATION=OFF \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_TESTING=OFF \ + -DVTK_USE_BOOST=ON \ + -DVTK_USE_CHARTS=ON \ + -DVTK_USE_VIEWS=ON \ + -DVTK_USE_RENDERING=ON \ + -DVTK_USE_CHEMISTRY=OFF \ + -DVTK_USE_HYBRID=OFF \ + -DVTK_USE_PARALLEL=OFF \ + -DVTK_USE_PATENTED=OFF \ + -DVTK_USE_INFOVIS=ON \ + -DVTK_USE_GL2PS=OFF \ + -DVTK_USE_MYSQL=OFF \ + -DVTK_USE_FFMPEG_ENCODER=OFF \ + -DVTK_USE_TEXT_ANALYSIS=OFF \ + -DVTK_WRAP_JAVA=OFF \ + -DVTK_WRAP_PYTHON=OFF \ + -DVTK_WRAP_TCL=OFF \ + -DVTK_USE_QT=OFF \ + -DVTK_USE_GUISUPPORT=OFF \ + -DVTK_USE_SYSTEM_ZLIB=ON \ + -DCMAKE_CXX_FLAGS="-D__STDC_CONSTANT_MACROS" + make -j2 && make install && touch ${config} + return $? +} + +function install_qhull() +{ + local pkg_ver=2012.1 + local pkg_file="qhull-${pkg_ver}" + local pkg_url="http://www.qhull.org/download/${pkg_file}-src.tgz" + local pkg_md5sum="d0f978c0d8dfb2e919caefa56ea2953c" + local QHULL_DIR=$HOME/qhull + local announce=$QHULL_DIR/share/doc/qhull/Announce.txt + echo "Installing QHull ${pkg_ver}" + if [[ -d $QHULL_DIR ]]; then + if [[ -e ${announce} ]]; then + local version=`grep -Po "(?<=Qhull )[0-9.]*(?= )" ${announce}` + if [[ "$version" = "$pkg_ver" ]]; then + local modified=`stat -c %y ${announce} | cut -d ' ' -f1` + echo " > Found cached installation of QHull" + echo " > Version ${pkg_ver}, built on ${modified}" + return 0 + fi + fi + fi + download ${pkg_url} ${pkg_md5sum} + if [[ $? -ne 0 ]]; then + return $? + fi + tar xzf pkg + cd ${pkg_file} + mkdir -p build && cd build + cmake .. \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_CXX_FLAGS=-fPIC \ + -DCMAKE_C_FLAGS=-fPIC \ + -DCMAKE_INSTALL_PREFIX=$QHULL_DIR + make -j2 && make install && touch ${announce} + return $? +} + +function install_doxygen() +{ + local pkg_ver=1.8.9.1 + local pkg_file="doxygen-${pkg_ver}" + local pkg_url="http://ftp.stack.nl/pub/users/dimitri/${pkg_file}.src.tar.gz" + local pkg_md5sum="3d1a5c26bef358c10a3894f356a69fbc" + local DOXYGEN_EXE=$DOXYGEN_DIR/bin/doxygen + echo "Installing Doxygen ${pkg_ver}" + if [[ -d $DOXYGEN_DIR ]]; then + if [[ -e $DOXYGEN_EXE ]]; then + local version=`$DOXYGEN_EXE --version` + if [[ "$version" = "$pkg_ver" ]]; then + local modified=`stat -c %y $DOXYGEN_EXE | cut -d ' ' -f1` + echo " > Found cached installation of Doxygen" + echo " > Version ${pkg_ver}, built on ${modified}" + return 0 + fi + fi + fi + download ${pkg_url} ${pkg_md5sum} + if [[ $? -ne 0 ]]; then + return $? + fi + tar xzf pkg + cd ${pkg_file} + ./configure --prefix $DOXYGEN_DIR + make -j2 && make install + return $? +} + +function install_dependencies() +{ + install_flann + install_vtk + install_qhull + install_doxygen +} + +function download() +{ + mkdir -p $DOWNLOAD_DIR && cd $DOWNLOAD_DIR && rm -rf * + wget --output-document=pkg $1 + if [[ $? -ne 0 ]]; then + return $? + fi + if [[ $# -ge 2 ]]; then + echo "$2 pkg" > "md5" + md5sum -c "md5" --quiet --status + if [[ $? -ne 0 ]]; then + echo "MD5 mismatch" + return 1 + fi + fi + return 0 +} + +case $1 in + install ) install_dependencies;; build ) build;; test ) test;; doc ) doc;; diff --git a/.travis.yml b/.travis.yml index f2562b5e..b7d2204c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,3 +1,4 @@ +sudo: false language: cpp compiler: - gcc @@ -31,16 +32,30 @@ env: - secure: PZivWbaCWFA2BFFY8n3UMxdEWjz7rBh568u9LF5LH3HgWADnfiwWzNriACqX9fhe7tSmDru5Bk978s+xPPAY9v24cfiDEX5a5MQ/XVr2rP48n3vlUDWERDhIodJ73F9F9GGZXToGdNz0MBUAHgiv7Lb0GYUfmOYzUJjWghngLBw= matrix: include: - - compiler: clang + - compiler: gcc env: TASK="test" - env: TASK="doc" +addons: + apt: + sources: + - kalakris-cmake + - boost-latest + - kubuntu-backports + packages: + - cmake + - libboost1.55-all-dev + - libeigen3-dev + - libgtest-dev + - doxygen-latex + - dvipng + - libusb-1.0-0-dev +cache: + directories: + - $HOME/flann + - $HOME/vtk + - $HOME/qhull + - $HOME/doxygen before_install: - - sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl -y - - sudo add-apt-repository ppa:apokluda/boost1.53 -y - - sudo add-apt-repository ppa:yade-users/external -y - - sudo add-apt-repository ppa:libreoffice/ppa -y - - sudo apt-get update -d -install: - - sudo apt-get install libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev libboost-chrono1.53-dev libusb-1.0-0-dev libgtest-dev + - bash .travis.sh install script: - - bash .travis.sh + - bash .travis.sh $TASK diff --git a/2d/CMakeLists.txt b/2d/CMakeLists.txt new file mode 100644 index 00000000..967a3008 --- /dev/null +++ b/2d/CMakeLists.txt @@ -0,0 +1,48 @@ +set(SUBSYS_NAME 2d) +set(SUBSYS_DESC "Point cloud 2d") +set(SUBSYS_DEPS common io filters) + +set(build TRUE) +PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS vtk) + +PCL_ADD_DOC("${SUBSYS_NAME}") + +if(build) + + set(srcs + src/convolution_2d.cpp + ) + + set(incs + "include/pcl/${SUBSYS_NAME}/convolution.h" + "include/pcl/${SUBSYS_NAME}/kernel.h" + "include/pcl/${SUBSYS_NAME}/edge.h" + "include/pcl/${SUBSYS_NAME}/morphology.h" + ) + + set(impl_incs + "include/pcl/${SUBSYS_NAME}/impl/convolution.hpp" + "include/pcl/${SUBSYS_NAME}/impl/edge.hpp" + "include/pcl/${SUBSYS_NAME}/impl/morphology.hpp" + ) + + set(LIB_NAME "pcl_${SUBSYS_NAME}") + + if(${VTK_FOUND}) + set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") + include("${VTK_USE_FILE}") + set(VTK_IO_TARGET_LINK_LIBRARIES vtkCommon vtkWidgets vtkIO vtkImaging) + endif(${VTK_FOUND}) + + include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${VTK_INCLUDE_DIRECTORIES}) + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${impl_incs}) + link_directories(${VTK_LINK_DIRECTORIES}) + target_link_libraries("${LIB_NAME}" ${VTK_LIBRARIES} pcl_io) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "" "" "" "") + + #Install include files + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) + +endif(build) diff --git a/2d/include/pcl/2d/convolution.h b/2d/include/pcl/2d/convolution.h new file mode 100644 index 00000000..7938e2b0 --- /dev/null +++ b/2d/include/pcl/2d/convolution.h @@ -0,0 +1,159 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_CONVOLUTION_H +#define PCL_2D_CONVOLUTION_H + +#include +#include +#include + +namespace pcl +{ + /** + * This typedef is used to represent a point cloud containing edge information + */ + struct PointXYZIEdge + { + PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding + float magnitude; + float direction; + float magnitude_x; + float magnitude_y; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned + } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment + + /** \brief A 2D convolution class. */ + template + class Convolution : public Filter + { + public: + using Filter::input_; + + /** + * Extra pixels are added to the input image so that convolution can be performed over the entire image. + * + * (kernel_height/2) rows are added before the first row and after the last row + * (kernel_width/2) columns are added before the first column and after the last column + * border options define what values are set for these extra rows and columns + * + * Assume that the three rows of right edge of the image looks like this: + * .. 3 2 1 + * .. 6 5 4 + * .. 9 8 7 + * + * BOUNDARY_OPTION_CLAMP : the extra pixels are set to the pixel value of the boundary pixel + * This option makes it seem as if it were: + * .. 3 2 1| 1 1 1 .. + * .. 6 5 4| 4 4 4 .. + * .. 9 8 7| 7 7 7 .. + * + * BOUNDARY_OPTION_MIRROR : the input image is mirrored at the boundary. + * This option makes it seem as if it were: + * .. 3 2 1| 1 2 3 .. + * .. 6 5 4| 4 5 6 .. + * .. 9 8 7| 7 8 9 .. + * + * BOUNDARY_OPTION_ZERO_PADDING : the extra pixels are simply set to 0 + * This option makes it seem as if it were: + * .. 3 2 1| 0 0 0 .. + * .. 6 5 4| 0 0 0 .. + * .. 9 8 7| 0 0 0 .. + * + * Note that the input image is not actually extended in size. Instead, based on these options, + * the convolution is performed differently at the border pixels. + */ + enum BOUNDARY_OPTIONS_ENUM + { + BOUNDARY_OPTION_CLAMP, + BOUNDARY_OPTION_MIRROR, + BOUNDARY_OPTION_ZERO_PADDING + }; + + Convolution () + { + boundary_options_ = BOUNDARY_OPTION_CLAMP; + } + + /** \brief Sets the kernel to be used for convolution + * \param[in] kernel convolution kernel passed by reference + */ + inline void + setKernel (const pcl::PointCloud &kernel) + { + kernel_ = kernel; + } + + /** + * \param[in] boundary_options enum indicating the boundary options to be used for convolution + */ + inline void + setBoundaryOptions (BOUNDARY_OPTIONS_ENUM boundary_options) + { + boundary_options_ = boundary_options; + } + + /** \brief Performs 2D convolution of the input point cloud with the kernel. + * Uses clamp as the default boundary option. + * \param[out] output Output point cloud passed by reference + */ + void + filter (pcl::PointCloud &output); + + protected: + /** \brief This is an over-ride function for the pcl::Filter interface. */ + void + applyFilter (pcl::PointCloud &) {} + + private: + BOUNDARY_OPTIONS_ENUM boundary_options_; + pcl::PointCloud kernel_; + }; +} + +#include + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZIEdge, + (float, x, x) + (float, y, y) + (float, z, z) + (float, magnitude, magnitude) + (float, direction, direction) + (float, magnitude_x, magnitude_x) + (float, magnitude_y, magnitude_y) +) +#endif // PCL_2D_CONVOLUTION_2D_H diff --git a/2d/include/pcl/2d/edge.h b/2d/include/pcl/2d/edge.h new file mode 100644 index 00000000..78b1af36 --- /dev/null +++ b/2d/include/pcl/2d/edge.h @@ -0,0 +1,306 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_EDGE_H +#define PCL_2D_EDGE_H + +#include +#include +#include + +namespace pcl +{ + template + class Edge + { + private: + typedef typename pcl::PointCloud PointCloudIn; + typedef typename PointCloudIn::Ptr PointCloudInPtr; + + PointCloudInPtr input_; + pcl::Convolution convolution_; + kernel kernel_; + + /** \brief This function performs edge tracing for Canny Edge detector. + * + * \param[in] rowOffset row offset for direction in which the edge is to be traced + * \param[in] colOffset column offset for direction in which the edge is to be traced + * \param[in] row row location of the edge point + * \param[in] col column location of the edge point + * \param[out] maxima point cloud containing the edge information in the magnitude channel + */ + inline void + cannyTraceEdge (int rowOffset, int colOffset, int row, int col, + pcl::PointCloud &maxima); + + /** \brief This function discretizes the edge directions in steps of 22.5 degrees. + * \param thet point cloud containing the edge information in the direction channel + */ + void + discretizeAngles (pcl::PointCloud &thet); + + /** \brief This function suppresses the edges which don't form a local maximum + * in the edge direction. + * \param[in] edges point cloud containing all the edges + * \param[out] maxima point cloud containing the non-max supressed edges + * \param[in] tLow + */ + void + suppressNonMaxima (const pcl::PointCloud &edges, + pcl::PointCloud &maxima, float tLow); + + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + enum OUTPUT_TYPE + { + OUTPUT_Y, + OUTPUT_X, + OUTPUT_X_Y, + OUTPUT_MAGNITUDE, + OUTPUT_DIRECTION, + OUTPUT_MAGNITUDE_DIRECTION, + OUTPUT_ALL + }; + + enum DETECTOR_KERNEL_TYPE + { + CANNY, + SOBEL, + PREWITT, + ROBERTS, + LOG, + DERIVATIVE_CENTRAL, + DERIVATIVE_FORWARD, + DERIVATIVE_BACKWARD + }; + + private: + OUTPUT_TYPE output_type_; + DETECTOR_KERNEL_TYPE detector_kernel_type_; + bool non_maximal_suppression_; + bool hysteresis_thresholding_; + + float hysteresis_threshold_low_; + float hysteresis_threshold_high_; + float non_max_suppression_radius_x_; + float non_max_suppression_radius_y_; + + public: + Edge () : + output_type_ (OUTPUT_X), + detector_kernel_type_ (SOBEL), + non_maximal_suppression_ (false), + hysteresis_thresholding_ (false), + hysteresis_threshold_low_ (20), + hysteresis_threshold_high_ (80), + non_max_suppression_radius_x_ (3), + non_max_suppression_radius_y_ (3) + { + } + + /** \brief Set the output type. + * \param[in] output_type the output type + */ + void + setOutputType (OUTPUT_TYPE output_type) + { + output_type_ = output_type; + } + + void + setHysteresisThresholdLow (float threshold) + { + hysteresis_threshold_low_ = threshold; + } + + void + setHysteresisThresholdHigh (float threshold) + { + hysteresis_threshold_high_ = threshold; + } + + /** + * \param[in] input_x + * \param[in] input_y + * \param[out] output + */ + void + sobelMagnitudeDirection (const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output); + + + /** \brief Perform Canny edge detection with two separated input images for + * horizontal and vertical derivatives. + * All edges of magnitude above t_high are always classified as edges. All edges + * below t_low are discarded. Edge values between t_low and t_high are classified + * as edges only if they are connected to edges having magnitude > t_high and are + * located in a direction perpendicular to that strong edge. + * + * \param[in] input_x Input point cloud passed by reference for the first derivative in the horizontal direction + * \param[in] input_y Input point cloud passed by reference for the first derivative in the vertical direction + * \param[out] output Output point cloud passed by reference + */ + void + canny (const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output); + + /** \brief This is a convenience function which performs edge detection based on + * the variable detector_kernel_type_ + * \param[out] output + */ + void + detectEdge (pcl::PointCloud &output); + + /** \brief All edges of magnitude above t_high are always classified as edges. + * All edges below t_low are discarded. + * Edge values between t_low and t_high are classified as edges only if they are + * connected to edges having magnitude > t_high and are located in a direction + * perpendicular to that strong edge. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeCanny (pcl::PointCloud &output); + + /** \brief Uses the Sobel kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeSobel (pcl::PointCloud &output); + + /** \brief Uses the Prewitt kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgePrewitt (pcl::PointCloud &output); + + /** \brief Uses the Roberts kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeRoberts (pcl::PointCloud &output); + + /** \brief Uses the LoG kernel for edge detection. + * Zero crossings of the Laplacian operator applied on an image indicate edges. + * Gaussian kernel is used to smoothen the image prior to the Laplacian. + * This is because Laplacian uses the second order derivative of the image and hence, is very sensitive to noise. + * The implementation is not two-step but rather applies the LoG kernel directly. + * + * \param[in] kernel_sigma variance of the LoG kernel used. + * \param[in] kernel_size a LoG kernel of dimensions kernel_size x kernel_size is used. + * \param[out] output Output point cloud passed by reference. + */ + void + detectEdgeLoG (const float kernel_sigma, const float kernel_size, + pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYCentralKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXCentral (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYCentralKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYCentral (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYForwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXForward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYForwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYForward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeXBackwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param output Output point cloud passed by reference + */ + void + computeDerivativeXBackward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYBackwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYBackward (pcl::PointCloud &output); + + /** \brief Override function to implement the pcl::Filter interface + */ + void + applyFilter (pcl::PointCloud& /*output*/) {} + + /** \brief Set the input point cloud pointer + * \param[in] input pointer to input point cloud + */ + void + setInputCloud (PointCloudInPtr input) + { + input_ = input; + } + }; +} +#include + +#endif // PCL_2D_EDGE_H + diff --git a/2d/include/pcl/2d/impl/convolution.hpp b/2d/include/pcl/2d/impl/convolution.hpp new file mode 100644 index 00000000..0ded38e8 --- /dev/null +++ b/2d/include/pcl/2d/impl/convolution.hpp @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_CONVOLUTION_IMPL_HPP +#define PCL_2D_CONVOLUTION_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Convolution::filter (pcl::PointCloud &output) +{ + int input_row = 0; + int input_col = 0; + // default boundary option : zero padding + output = *input_; + + int iw = static_cast (input_->width), + ih = static_cast (input_->height), + kw = static_cast (kernel_.width), + kh = static_cast (kernel_.height); + switch (boundary_options_) + { + default: + case BOUNDARY_OPTION_CLAMP: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = 0; + else if (ikkh >= ih) + input_row = ih - 1; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = 0; + else if (jlkw >= iw) + input_col = iw - 1; + else + input_col = jlkw; + + intensity += kernel_ (l, k).intensity * (*input_)(input_col, input_row).intensity; + } + } + output (j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_MIRROR: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = -ikkh - 1; + else if (ikkh >= ih) + input_row = 2 * ih - 1 - ikkh; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = -jlkw - 1; + else if (jlkw >= iw) + input_col = 2 * iw - 1 - jlkw; + else + input_col = jlkw; + + intensity += kernel_ (l, k).intensity * ((*input_)(input_col, input_row).intensity); + } + } + output (j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_ZERO_PADDING: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0 || ikkh >= ih || jlkw < 0 || jlkw >= iw) + continue; + else + intensity += kernel_ (l, k).intensity * ((*input_)(jlkw, ikkh).intensity); + } + } + output (j, i).intensity = intensity; + } + } + break; + } + } // switch +} + +#endif diff --git a/2d/include/pcl/2d/impl/edge.hpp b/2d/include/pcl/2d/impl/edge.hpp new file mode 100644 index 00000000..24513545 --- /dev/null +++ b/2d/include/pcl/2d/impl/edge.hpp @@ -0,0 +1,494 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_EDGE_IMPL_HPP +#define PCL_2D_EDGE_IMPL_HPP + +#include +#include // rad2deg() +#include + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeSobel ( + pcl::PointCloud &output) +{ + //pcl::console::TicToc tt; + //tt.tic (); + convolution_.setInputCloud (input_); + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + //PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic (); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + //PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic (); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } + //PCL_ERROR ("Rest: %g\n", tt.toc ()); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::sobelMagnitudeDirection ( + const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_x.makeShared()); + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + convolution_.setInputCloud (input_y.makeShared()); + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_x.height; + const int width = input_x.width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgePrewitt (pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::PREWITT_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::PREWITT_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeRoberts (pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::ROBERTS_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::ROBERTS_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::cannyTraceEdge ( + int rowOffset, int colOffset, int row, int col, + pcl::PointCloud &maxima) +{ + int newRow = row + rowOffset; + int newCol = col + colOffset; + PointXYZI &pt = maxima (newCol, newRow); + + if (newRow > 0 && newRow < static_cast (maxima.height) && newCol > 0 && newCol < static_cast (maxima.width)) + { + if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits::max ()) + return; + + pt.intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, newRow, newCol, maxima); + cannyTraceEdge (-1, 0, newRow, newCol, maxima); + cannyTraceEdge ( 1, 1, newRow, newCol, maxima); + cannyTraceEdge (-1, -1, newRow, newCol, maxima); + cannyTraceEdge ( 0, -1, newRow, newCol, maxima); + cannyTraceEdge ( 0, 1, newRow, newCol, maxima); + cannyTraceEdge (-1, 1, newRow, newCol, maxima); + cannyTraceEdge ( 1, -1, newRow, newCol, maxima); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::discretizeAngles (pcl::PointCloud &thet) +{ + const int height = thet.height; + const int width = thet.width; + float angle; + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + angle = pcl::rad2deg (thet (j, i).direction); + if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5)) + thet (j, i).direction = 0; + else + if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5))) + thet (j, i).direction = 45; + else + if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5))) + thet (j, i).direction = 90; + else + if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5))) + thet (j, i).direction = 135; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::suppressNonMaxima ( + const pcl::PointCloud &edges, + pcl::PointCloud &maxima, float tLow) +{ + const int height = edges.height; + const int width = edges.width; + + maxima.height = height; + maxima.width = width; + maxima.resize (height * width); + + for (size_t i = 0; i < maxima.size (); ++i) + maxima[i].intensity = 0.0f; + + // tHigh and non-maximal supression + for (int i = 1; i < height - 1; i++) + { + for (int j = 1; j < width - 1; j++) + { + const PointXYZIEdge &ptedge = edges (j, i); + PointXYZI &ptmax = maxima (j, i); + + if (ptedge.magnitude < tLow) + continue; + + //maxima (j, i).intensity = 0; + + switch (int (ptedge.direction)) + { + case 0: + { + if (ptedge.magnitude >= edges (j - 1, i).magnitude && + ptedge.magnitude >= edges (j + 1, i).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 45: + { + if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude && + ptedge.magnitude >= edges (j + 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 90: + { + if (ptedge.magnitude >= edges (j, i - 1).magnitude && + ptedge.magnitude >= edges (j, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 135: + { + if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude && + ptedge.magnitude >= edges (j - 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeCanny (pcl::PointCloud &output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + //pcl::console::TicToc tt; + //tt.tic (); + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel (new pcl::PointCloud); + PointCloudInPtr smoothed_cloud (new PointCloudIn); + kernel_.setKernelSize (3); + kernel_.setKernelSigma (1.0); + kernel_.setKernelType (kernel::GAUSSIAN); + kernel_.fetchKernel (*gaussian_kernel); + convolution_.setKernel (*gaussian_kernel); + convolution_.setInputCloud (input_); + convolution_.filter (*smoothed_cloud); + //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic (); + + // Edge detection usign Sobel + pcl::PointCloud::Ptr edges (new pcl::PointCloud); + setInputCloud (smoothed_cloud); + detectEdgeSobel (*edges); + //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic (); + + // Edge discretization + discretizeAngles (*edges); + //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic (); + + // tHigh and non-maximal supression + pcl::PointCloud::Ptr maxima (new pcl::PointCloud); + suppressNonMaxima (*edges, *maxima, tLow); + //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic (); + + // Edge tracing + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits::max ()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, i, j, *maxima); + cannyTraceEdge (-1, 0, i, j, *maxima); + cannyTraceEdge ( 1, 1, i, j, *maxima); + cannyTraceEdge (-1, -1, i, j, *maxima); + cannyTraceEdge ( 0, -1, i, j, *maxima); + cannyTraceEdge ( 0, 1, i, j, *maxima); + cannyTraceEdge (-1, 1, i, j, *maxima); + cannyTraceEdge ( 1, -1, i, j, *maxima); + } + } + //PCL_ERROR ("Edge tracing: %g\n", tt.toc ()); + + // Final thresholding + for (size_t i = 0; i < input_->size (); ++i) + { + if ((*maxima)[i].intensity == std::numeric_limits::max ()) + output[i].magnitude = 255; + else + output[i].magnitude = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::canny ( + const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_x.height; + const int width = input_x.width; + + output.resize (height * width); + output.height = height; + output.width = width; + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel (new pcl::PointCloud); + kernel_.setKernelSize (3); + kernel_.setKernelSigma (1.0); + kernel_.setKernelType (kernel::GAUSSIAN); + kernel_.fetchKernel (*gaussian_kernel); + convolution_.setKernel (*gaussian_kernel); + + PointCloudIn smoothed_cloud_x; + convolution_.setInputCloud (input_x.makeShared()); + convolution_.filter (smoothed_cloud_x); + + PointCloudIn smoothed_cloud_y; + convolution_.setInputCloud (input_y.makeShared()); + convolution_.filter (smoothed_cloud_y); + + + // Edge detection usign Sobel + pcl::PointCloud::Ptr edges (new pcl::PointCloud); + sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ()); + + // Edge discretization + discretizeAngles (*edges); + + pcl::PointCloud::Ptr maxima (new pcl::PointCloud); + suppressNonMaxima (*edges, *maxima, tLow); + + // Edge tracing + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits::max ()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, i, j, *maxima); + cannyTraceEdge (-1, 0, i, j, *maxima); + cannyTraceEdge ( 1, 1, i, j, *maxima); + cannyTraceEdge (-1, -1, i, j, *maxima); + cannyTraceEdge ( 0, -1, i, j, *maxima); + cannyTraceEdge ( 0, 1, i, j, *maxima); + cannyTraceEdge (-1, 1, i, j, *maxima); + cannyTraceEdge ( 1, -1, i, j, *maxima); + } + } + + // Final thresholding + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity == std::numeric_limits::max ()) + output (j, i).magnitude = 255; + else + output (j, i).magnitude = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeLoG ( + const float kernel_sigma, const float kernel_size, + pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr log_kernel (new pcl::PointCloud); + kernel_.setKernelType (kernel::LOG); + kernel_.setKernelSigma (kernel_sigma); + kernel_.setKernelSize (kernel_size); + kernel_.fetchKernel (*log_kernel); + convolution_.setKernel (*log_kernel); + convolution_.filter (output); +} + +#endif diff --git a/2d/include/pcl/2d/impl/kernel.hpp b/2d/include/pcl/2d/impl/kernel.hpp new file mode 100644 index 00000000..223fba8c --- /dev/null +++ b/2d/include/pcl/2d/impl/kernel.hpp @@ -0,0 +1,328 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_KERNEL_IMPL_HPP +#define PCL_2D_KERNEL_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::fetchKernel (pcl::PointCloud &kernel) +{ + switch (kernel_type_) + { + case SOBEL_X: + { + sobelKernelX (kernel); + break; + } + case SOBEL_Y: + { + sobelKernelY (kernel); + break; + } + case PREWITT_X: + { + prewittKernelX (kernel); + break; + } + case PREWITT_Y: + { + prewittKernelY (kernel); + break; + } + case ROBERTS_X: + { + robertsKernelX (kernel); + break; + } + case ROBERTS_Y: + { + robertsKernelY (kernel); + break; + } + case LOG: + { + loGKernel (kernel); + break; + } + case DERIVATIVE_CENTRAL_X: + { + derivativeXCentralKernel (kernel); + break; + } + case DERIVATIVE_FORWARD_X: + { + derivativeXForwardKernel (kernel); + break; + } + case DERIVATIVE_BACKWARD_X: + { + derivativeXBackwardKernel (kernel); + break; + } + case DERIVATIVE_CENTRAL_Y: + { + derivativeYCentralKernel (kernel); + break; + } + case DERIVATIVE_FORWARD_Y: + { + derivativeYForwardKernel (kernel); + break; + } + case DERIVATIVE_BACKWARD_Y: + { + derivativeYBackwardKernel (kernel); + break; + } + case GAUSSIAN: + { + gaussianKernel (kernel); + break; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::gaussianKernel (pcl::PointCloud &kernel) +{ + float sum = 0; + kernel.resize (kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) + { + for (int j = 0; j < kernel_size_; j++) + { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + kernel (j, i).intensity = expf (float (- double (iks * iks + jks * jks) / sigma_sqr)); + sum += float (kernel (j, i).intensity); + } + } + + // Normalizing the kernel + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::loGKernel (pcl::PointCloud &kernel) +{ + float sum = 0; + float temp = 0; + kernel.resize (kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) + { + for (int j = 0; j < kernel_size_; j++) + { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + temp = float (double (iks * iks + jks * jks) / sigma_sqr); + kernel (j, i).intensity = (1.0f - temp) * expf (-temp); + sum += kernel (j, i).intensity; + } + } + + // Normalizing the kernel + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::sobelKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = -2; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 2; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::prewittKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 1; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::robertsKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (4); + kernel.height = 2; + kernel.width = 2; + kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 0; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = -1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::sobelKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = -2; kernel (2, 0).intensity = -1; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0; + kernel (0, 2).intensity = 1; kernel (1, 2).intensity = 2; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::prewittKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = -1; kernel (2, 2).intensity = -1; +} + +template void +pcl::kernel::robertsKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (4); + kernel.height = 2; + kernel.width = 2; + kernel (0, 0).intensity = 0; kernel (1, 0).intensity = 1; + kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXCentralKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXForwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = 0; kernel (1, 0).intensity = -1; kernel (2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXBackwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYCentralKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 0; kernel (0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYForwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = 0; kernel (0, 1).intensity = -1; kernel (0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYBackwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 1; kernel (0, 2).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelType (KERNEL_ENUM kernel_type) +{ + kernel_type_ = kernel_type; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelSize (int kernel_size) +{ + kernel_size_ = kernel_size; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelSigma (float kernel_sigma) +{ + sigma_ = kernel_sigma; +} + + +#endif diff --git a/2d/include/pcl/2d/impl/keypoint.hpp b/2d/include/pcl/2d/impl/keypoint.hpp new file mode 100644 index 00000000..2a99ab56 --- /dev/null +++ b/2d/include/pcl/2d/impl/keypoint.hpp @@ -0,0 +1,247 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * keypoint.hpp + * + * Created on: May 28, 2012 + * Author: somani + */ + +#ifndef PCL_2D_KEYPOINT_HPP_ +#define PCL_2D_KEYPOINT_HPP_ + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh){ + + /*creating the gaussian kernels*/ + ImageType kernel_d; + ImageType kernel_i; + conv_2d.gaussianKernel (5, sigma_d, kernel_d); + conv_2d.gaussianKernel (5, sigma_i, kernel_i); + + /*scaling the image with differentiation scale*/ + ImageType smoothed_image; + conv_2d.convolve (smoothed_image, kernel_d, input); + + /*image derivatives*/ + ImageType I_x, I_y; + edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image); + edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image); + + /*second moment matrix*/ + ImageType I_x2, I_y2, I_xI_y; + imageElementMultiply (I_x2, I_x, I_x); + imageElementMultiply (I_y2, I_y, I_y); + imageElementMultiply (I_xI_y, I_x, I_y); + + /*scaling second moment matrix with integration scale*/ + ImageType M00, M10, M11; + conv_2d.convolve (M00, kernel_i, I_x2); + conv_2d.convolve (M10, kernel_i, I_xI_y); + conv_2d.convolve (M11, kernel_i, I_y2); + + /*harris function*/ + const size_t height = input.size (); + const size_t width = input[0].size (); + output.resize (height); + for (size_t i = 0; i < height; i++) + { + output[i].resize (width); + for (size_t j = 0; j < width; j++) + { + output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) - alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j])); + if (thresh != 0) + { + if (output[i][j] < thresh) + output[i][j] = 0; + else + output[i][j] = 255; + } + } + } + + /*local maxima*/ + for (size_t i = 1; i < height - 1; i++) + { + for (size_t j = 1; j < width - 1; j++) + { + if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] && output[i][j] > output[i - 1][j + 1] && + output[i][j] > output[i][j - 1] && output[i][j] > output[i][j + 1] && + output[i][j] > output[i + 1][j - 1] && output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1]) + ; + else + output[i][j] = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALED){ + /*creating the gaussian kernels*/ + ImageType kernel, cornerness; + conv_2d.gaussianKernel (5, sigma, kernel); + + /*scaling the image with differentiation scale*/ + ImageType smoothed_image; + conv_2d.convolve (smoothed_image, kernel, input); + + /*image derivatives*/ + ImageType I_x, I_y; + edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image); + edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image); + + /*second moment matrix*/ + ImageType I_xx, I_yy, I_xy; + edge_detection.ComputeDerivativeXCentral (I_xx, I_x); + edge_detection.ComputeDerivativeYCentral (I_xy, I_x); + edge_detection.ComputeDerivativeYCentral (I_yy, I_y); + /*Determinant of Hessian*/ + const size_t height = input.size (); + const size_t width = input[0].size (); + float min = std::numeric_limits::max(); + float max = std::numeric_limits::min(); + cornerness.resize (height); + for (size_t i = 0; i < height; i++) + { + cornerness[i].resize (width); + for (size_t j = 0; j < width; j++) + { + cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]); + if(SCALED){ + if(cornerness[i][j] < min) + min = cornerness[i][j]; + if(cornerness[i][j] > max) + max = cornerness[i][j]; + } + } + + /*local maxima*/ + output.resize (height); + output[0].resize (width); + output[height-1].resize (width); + for (size_t i = 1; i < height - 1; i++) + { + output[i].resize (width); + for (size_t j = 1; j < width - 1; j++) + { + if(SCALED) + output[i][j] = ((cornerness[i][j]-min)/(max-min)); + else + output[i][j] = cornerness[i][j]; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales){ + const size_t height = input.size(); + const size_t width = input[0].size(); + const int local_search_radius = 1; + float scale = start_scale; + std::vector cornerness; + cornerness.resize(num_scales); + for(int i = 0;i < num_scales;i++){ + hessianBlob(cornerness[i], input, scale, false); + scale *= scaling_factor; + } + bool non_max_flag = false; + float scale_max, local_max; + for(size_t i = 0;i < height;i++){ + for(size_t j = 0;j < width;j++){ + scale_max = std::numeric_limits::min(); + /*default output in case of no blob at the current point is 0*/ + output[i][j] = 0; + for(int k = 0;k < num_scales;k++){ + /*check if the current point (k,i,j) is a maximum in the defined search radius*/ + non_max_flag = false; + local_max = cornerness[k][i][j]; + for(int n = -local_search_radius; n <= local_search_radius;n++){ + if(n+k < 0 || n+k >= num_scales) + continue; + for(int l = -local_search_radius;l <= local_search_radius;l++){ + if(l+i < 0 || l+i >= height) + continue; + for(int m = -local_search_radius; m <= local_search_radius;m++){ + if(m+j < 0 || m+j >= width) + continue; + if(cornerness[n+k][l+i][m+j] > local_max){ + non_max_flag = true; + break; + } + } + if(non_max_flag) + break; + } + if(non_max_flag) + break; + } + /*if the current point is a point of local maximum, check if it is a maximum point across scales*/ + if(!non_max_flag){ + if(cornerness[k][i][j] > scale_max){ + scale_max = cornerness[k][i][j]; + /*output indicates the scale at which the blob is found at the current location in the image*/ + output[i][i] = start_scale*pow(scaling_factor, k); + } + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2){ + const size_t height = input1.size (); + const size_t width = input1[0].size (); + output.resize (height); + for (size_t i = 0; i < height; i++) + { + output[i].resize (width); + for (size_t j = 0; j < width; j++) + { + output[i][j] = input1[i][j] * input2[i][j]; + } + } +} + +#endif // PCL_2D_KEYPOINT_HPP_ diff --git a/2d/include/pcl/2d/impl/morphology.hpp b/2d/include/pcl/2d/impl/morphology.hpp new file mode 100644 index 00000000..6528eed9 --- /dev/null +++ b/2d/include/pcl/2d/impl/morphology.hpp @@ -0,0 +1,380 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_MORPHOLOGY_HPP_ +#define PCL_2D_MORPHOLOGY_HPP_ + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::erosionBinary (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool mismatch_flag; + + output.width = width; + output.height = height; + output.resize (width * height); + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + // Operation done only at 1's + if ((*input_)(j, i).intensity == 0) + { + output (j, i).intensity = 0; + continue; + } + mismatch_flag = false; + for (int k = 0; k < kernel_height; k++) + { + if (mismatch_flag) + break; + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity != 1) + { + output (j, i).intensity = 0; + mismatch_flag = true; + break; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = (mismatch_flag) ? 0 : 1; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::dilationBinary (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool match_flag; + + output.width = width; + output.height = height; + output.resize (width * height); + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + match_flag = false; + for (int k = 0; k < kernel_height; k++) + { + if (match_flag) + break; + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= height) + { + continue; + } + // If any position where kernel is 1 and image is also one is detected, + // matching occurs + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity == 1) + { + match_flag = true; + break; + } + } + } + // Assign value according to match flag + output (j, i).intensity = (match_flag) ? 1 : 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::openingBinary (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + erosionBinary (*intermediate_output); + this->setInputCloud (intermediate_output); + dilationBinary (output); +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::closingBinary (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + dilationBinary (*intermediate_output); + this->setInputCloud (intermediate_output); + erosionBinary (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::erosionGray (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float min; + output.resize (width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + min = -1; + for (int k = 0; k < kernel_height; k++) + { + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity < min || min == -1) + { + min = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = min; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::dilationGray (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float max; + + output.resize (width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + max = -1; + for (int k = 0; k < kernel_height; k++) + { + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity > max || max == -1) + { + max = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = max; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::openingGray (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + erosionGray (*intermediate_output); + this->setInputCloud (intermediate_output); + dilationGray (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::closingGray (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + dilationGray (*intermediate_output); + this->setInputCloud (intermediate_output); + erosionGray (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::subtractionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.hieght) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 && input2[i].intensity == 0) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::unionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.hieght) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 || input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::intersectionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.height) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 && input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::structuringElementCircular ( + pcl::PointCloud &kernel, const int radius) +{ + const int dim = 2 * radius; + kernel.height = dim; + kernel.width = dim; + kernel.resize (dim * dim); + + for (int i = 0; i < dim; i++) + { + for (int j = 0; j < dim; j++) + { + if (((i - radius) * (i - radius) + (j - radius) * (j - radius)) < radius * radius) + kernel (j, i).intensity = 1; + else + kernel (j, i).intensity = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::structuringElementRectangle ( + pcl::PointCloud &kernel, const int height, const int width) +{ + kernel.height = height; + kernel.width = width; + kernel.resize (height * width); + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::setStructuringElement (const PointCloudInPtr &structuring_element) +{ + structuring_element_ = structuring_element; +} + +#endif // PCL_2D_MORPHOLOGY_HPP_ diff --git a/2d/include/pcl/2d/kernel.h b/2d/include/pcl/2d/kernel.h new file mode 100644 index 00000000..e440933b --- /dev/null +++ b/2d/include/pcl/2d/kernel.h @@ -0,0 +1,244 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_KERNEL_H_ +#define PCL_2D_KERNEL_H_ +#include +#include +namespace pcl +{ + template + class kernel + { + public: + + /** + * enumerates the different types of kernels available. + */ + enum KERNEL_ENUM + { + SOBEL_X, //!< SOBEL_X + SOBEL_Y, //!< SOBEL_Y + PREWITT_X, //!< PREWITT_X + PREWITT_Y, //!< PREWITT_Y + ROBERTS_X, //!< ROBERTS_X + ROBERTS_Y, //!< ROBERTS_Y + LOG, //!< LOG + DERIVATIVE_CENTRAL_X, //!< DERIVATIVE_CENTRAL_X + DERIVATIVE_FORWARD_X, //!< DERIVATIVE_FORWARD_X + DERIVATIVE_BACKWARD_X,//!< DERIVATIVE_BACKWARD_X + DERIVATIVE_CENTRAL_Y, //!< DERIVATIVE_CENTRAL_Y + DERIVATIVE_FORWARD_Y, //!< DERIVATIVE_FORWARD_Y + DERIVATIVE_BACKWARD_Y,//!< DERIVATIVE_BACKWARD_Y + GAUSSIAN //!< GAUSSIAN + }; + + int kernel_size_; + float sigma_; + KERNEL_ENUM kernel_type_; + + kernel () : + kernel_size_ (3), + sigma_ (1.0), + kernel_type_ (GAUSSIAN) + { + + } + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Helper function which returns the kernel selected by the kernel_type_ enum + */ + void fetchKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_ + */ + + void gaussianKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Laplacian of Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_ + */ + + void loGKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the X direction + */ + + void sobelKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the X direction + */ + + void prewittKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the X direction + */ + + void robertsKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the Y direction + */ + + void sobelKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the Y direction + */ + + void prewittKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the Y direction + */ + + void robertsKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1] + */ + + void derivativeXCentralKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1]' + */ + + void derivativeYCentralKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1] + */ + + void derivativeXForwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1]' + */ + + void derivativeYForwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0] + */ + + void derivativeXBackwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0]' + */ + + void derivativeYBackwardKernel (PointCloud &kernel); + + /** + * + * @param kernel_type enum indicating the kernel type wanted + * + * select the kernel type. + */ + void setKernelType (KERNEL_ENUM kernel_type); + + /** + * + * @param kernel_size kernel of size kernel_size x kernel_size is created(LoG and Gaussian only) + * + * Setter function for kernel_size_ + */ + void setKernelSize (int kernel_size); + + /** + * + * @param kernel_sigma variance of the Gaussian or LoG kernels. + * + * Setter function for kernel_sigma_ + */ + void setKernelSigma (float kernel_sigma); + }; +} + +#include + +#endif // PCL_2D_KERNEL_H_ diff --git a/2d/include/pcl/2d/keypoint.h b/2d/include/pcl/2d/keypoint.h new file mode 100644 index 00000000..677e52e6 --- /dev/null +++ b/2d/include/pcl/2d/keypoint.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * keypoint.h + * + * Created on: May 28, 2012 + * Author: somani + */ + +#ifndef PCL_2D_KEYPOINT_H_ +#define PCL_2D_KEYPOINT_H_ + +#include + +namespace pcl +{ + class Keypoint + { + private: + Edge edge_detection; + Convolution conv_2d; + public: + Keypoint () + { + } + + void + harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh); + + void + hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALE); + + void + hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales); + + void + imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2); + }; +} + +#include + +#endif // PCL_2D_KEYPOINT_H_ diff --git a/2d/include/pcl/2d/morphology.h b/2d/include/pcl/2d/morphology.h new file mode 100644 index 00000000..a3847b45 --- /dev/null +++ b/2d/include/pcl/2d/morphology.h @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_MORPHOLOGY_H_ +#define PCL_2D_MORPHOLOGY_H_ + +#include + +namespace pcl +{ + template + class Morphology : public PCLBase + { + private: + typedef typename pcl::PointCloud PointCloudIn; + typedef typename PointCloudIn::Ptr PointCloudInPtr; + + PointCloudInPtr structuring_element_; + + public: + using PCLBase::input_; + + Morphology () {} + + /** \brief This function performs erosion followed by dilation. + * It is useful for removing noise in the form of small blobs and patches. + * \param[out] output Output point cloud passed by reference + */ + void + openingBinary (pcl::PointCloud &output); + + /** \brief This function performs dilation followed by erosion. + * It is useful for filling up (holes/cracks/small discontinuities) in a binary + * segmented region + * \param[out] output Output point cloud passed by reference + */ + void + closingBinary (pcl::PointCloud &output); + + /** \brief Binary dilation is similar to a logical disjunction of sets. + * At each pixel having value 1, if for all pixels in the structuring element having value 1, the corresponding + * pixels in the input image are also 1, the center pixel is set to 1. Otherwise, it is set to 0. + * \param[out] output Output point cloud passed by reference + */ + void + erosionBinary (pcl::PointCloud &output); + + /** \brief Binary erosion is similar to a logical addition of sets. + * At each pixel having value 1, if at least one pixel in the structuring element is 1 and the corresponding point + * in the input image is 1, the center pixel is set to 1. Otherwise, it is set to 0. + * \param[out] output Output point cloud passed by reference + */ + void + dilationBinary (pcl::PointCloud &output); + + /** \brief Grayscale erosion followed by dilation. + * This is used to remove small bright artifacts from the image. Large bright objects are relatively undisturbed. + * \param[out] output Output point cloud passed by reference + */ + void + openingGray (pcl::PointCloud &output); + + /** \brief Grayscale dilation followed by erosion. + * This is used to remove small dark artifacts from the image. Bright features or large dark features are relatively undisturbed. + * \param[out] output Output point cloud passed by reference + */ + void + closingGray (pcl::PointCloud &output); + + /** \brief Takes the min of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + erosionGray (pcl::PointCloud &output); + + /** \brief Takes the max of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + dilationGray (pcl::PointCloud &output); + + /** \brief Set operation + * output = input1 - input2 + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + subtractionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Set operation + * \f$output = input1 \cup input2\f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + unionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Set operation \f$ output = input1 \cap input2 \f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + intersectionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Creates a circular structing element. The size of the kernel created is 2*radius x 2*radius. + * Center of the structuring element is the center of the circle. + * All values lying on the circle are 1 and the others are 0. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] radius Radius of the circular structuring element. + */ + void + structuringElementCircular (pcl::PointCloud &kernel, const int radius); + + /** \brief Creates a rectangular structing element of size height x width. * + * All values are 1. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] height height number of rows in the structuring element + * \param[in] width number of columns in the structuring element + * + */ + void + structuringElementRectangle (pcl::PointCloud &kernel, + const int height, const int width); + + enum MORPHOLOGICAL_OPERATOR_TYPE + { + EROSION_GRAY, + DILATION_GRAY, + OPENING_GRAY, + CLOSING_GRAY, + EROSION_BINARY, + DILATION_BINARY, + OPENING_BINARY, + CLOSING_BINARY + }; + + MORPHOLOGICAL_OPERATOR_TYPE operator_type; + + /** + * \param[out] output Output point cloud passed by reference + */ + void + applyMorphologicalOperation (pcl::PointCloud &output); + + /** + * \param[in] structuring_element The structuring element to be used for the morphological operation + */ + void + setStructuringElement (const PointCloudInPtr &structuring_element); + }; +} + +#include + +#endif // PCL_2D_MORPHOLOGY_H_ diff --git a/2d/src/convolution_2d.cpp b/2d/src/convolution_2d.cpp new file mode 100644 index 00000000..460b3b42 --- /dev/null +++ b/2d/src/convolution_2d.cpp @@ -0,0 +1,38 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +// Instantiations of specific point types diff --git a/2d/src/examples.cpp b/2d/src/examples.cpp new file mode 100644 index 00000000..951cfcba --- /dev/null +++ b/2d/src/examples.cpp @@ -0,0 +1,134 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + */ + +#include + +#include +#include +#include +#include +#include + +using namespace pcl; + +void example_edge () +{ + Edge edge; + + /*dummy clouds*/ + pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); + + /*example 1*/ + edge.output_type_ = Edge::OUTPUT_X_Y; + edge.detectEdgeRoberts (*output_cloud, *input_cloud); + + /*example 2*/ + edge.hysteresis_threshold_low_ = 20; + edge.hysteresis_threshold_high_ = 80; + edge.non_max_suppression_radius_x_ = 3; + edge.non_max_suppression_radius_y_ = 3; + edge.detectEdgeCanny (*output_cloud, *input_cloud); + + /*example 3*/ + edge.detector_kernel_type_ = Edge::PREWITT; + edge.hysteresis_thresholding_ = true; + edge.hysteresis_threshold_low_ = 20; + edge.hysteresis_threshold_high_ = 80; + edge.non_maximal_suppression_ = true; + edge.non_max_suppression_radius_x_ = 1; + edge.non_max_suppression_radius_y_ = 1; + edge.output_type_ = Edge::OUTPUT_X_Y; + edge.detectEdge (*output_cloud, *input_cloud); +} + +void example_convolution () +{ + Kernel kernel; + Convolution convolution; + + /*dummy clouds*/ + pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr kernel_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); + + /*example 1 : Gaussian Smoothing*/ + kernel.sigma_ = 2.0; + kernel.kernel_size_ = 3; + kernel.gaussianKernel (*kernel_cloud); + convolution.kernel_ = *kernel_cloud; + convolution.convolve (*output_cloud, *input_cloud); + + /*example 2 : forward derivative in X direction*/ + kernel.kernel_type_ = Kernel::DERIVATIVE_FORWARD_X; + kernel.fetchKernel (*kernel_cloud); + convolution.kernel_ = *kernel_cloud; + convolution.convolve (*output_cloud, *input_cloud); + + /*example 3*/ + kernel.kernel_type_ = Kernel::DERIVATIVE_FORWARD_X; + kernel.fetchKernel (convolution.kernel_); + convolution.convolve (*output_cloud, *input_cloud); +} + +void example_morphology () +{ + Morphology morphology; + + /*dummy clouds*/ + pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr structuring_element_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr output_cloud (new pcl::PointCloud); + + /*example 1 : Gaussian Smoothing*/ + morphology.structuringElementCircular (*structuring_element_cloud, 3); + morphology.structuring_element_ = *structuring_element_cloud; + morphology.operator_type_ = Morphology::EROSION_GRAY; + morphology.applyMorphologicalOperation (*output_cloud, *input_cloud); + + /*example 2 : forward derivative in X direction*/ + morphology.structuringElementCircular (morphology.structuring_element_, 3); + morphology.operator_type_ = Morphology::EROSION_GRAY; + morphology.applyMorphologicalOperation (*output_cloud, *input_cloud); + +} + +int main(char *args, int argv) +{ + return 0; +} diff --git a/AUTHORS.txt b/AUTHORS.txt index c8501be2..bed9a7b5 100644 --- a/AUTHORS.txt +++ b/AUTHORS.txt @@ -1,4 +1,3 @@ -PCL is a large collaborative effort, and it would not exist without the -contributions of several people. Please see http://dev.pointclouds.org for a -complete list of developers. +PCL is a large collaborative effort, and it would not exist without the contributions of several people. + Please see https://github.com/PointCloudLibrary/pcl/graphs/contributors for a complete list of developers. diff --git a/CHANGES.md b/CHANGES.md index adc8e4ef..406951cd 100644 --- a/CHANGES.md +++ b/CHANGES.md @@ -1,5 +1,516 @@ # ChangeList +## *= 1.8.0 (14.06.2016) =* + +* Added missing `Eigen::aligned_allocator` in vectors and maps that contain + vectorizable Eigen where appropriate + [[#1034]](https://github.com/PointCloudLibrary/pcl/pull/1034) + [[#1052]](https://github.com/PointCloudLibrary/pcl/pull/1052) + [[#1068]](https://github.com/PointCloudLibrary/pcl/pull/1068) + [[#1182]](https://github.com/PointCloudLibrary/pcl/pull/1182) + [[#1497]](https://github.com/PointCloudLibrary/pcl/pull/1497) +* Fixed compilation errors/warning when compiling in C++11 mode + [[#1179]](https://github.com/PointCloudLibrary/pcl/pull/1179) +* Added a configuration option to choose between Qt4 and Qt5; the default is + changed to be Qt5 + [[#1217]](https://github.com/PointCloudLibrary/pcl/pull/1217) +* Improved compatibility with recent Eigen versions + [[#1261]](https://github.com/PointCloudLibrary/pcl/pull/1261) + [[#1298]](https://github.com/PointCloudLibrary/pcl/pull/1298) + [[#1316]](https://github.com/PointCloudLibrary/pcl/pull/1316) + [[#1369]](https://github.com/PointCloudLibrary/pcl/pull/1369) +* Added support for VTK compiled with OpenGL2 backend (was introduced in VTK + 6.3, became default in VTK 7.0) + [[#1534]](https://github.com/PointCloudLibrary/pcl/pull/1534) + +### `libpcl_common:` + +* Added `copy_all_fields` option to the family of transformPointCloudXXX() + functions + [[#805]](https://github.com/PointCloudLibrary/pcl/pull/805) +* Added a color lookup table consisting of 256 colors structured in a maximally + discontinuous manner (Glasbey colors) + [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849) +* Added a helper class `EventFrequency` to measure frequency of a certain event + [[#850]](https://github.com/PointCloudLibrary/pcl/pull/850) +* Added a new `UniqueShapeContext1960` point type + [[#856]](https://github.com/PointCloudLibrary/pcl/pull/856) +* Added a function `transformPointWithNormal()` + [[#908]](https://github.com/PointCloudLibrary/pcl/pull/908) +* Fixed index-out-of-range error in `copyPointCloud()` for empty clouds + [[#933]](https://github.com/PointCloudLibrary/pcl/pull/933) +* Fixed errors when compiling library with Boost 1.56 and Qt4 + [[#938]](https://github.com/PointCloudLibrary/pcl/pull/938) +* Created a new point type `PointXYZLNormal` with position, normal, and label + fields + [[#962]](https://github.com/PointCloudLibrary/pcl/pull/962) +* Created a new point type `PointDEM` to represent Digital Elevation Maps + [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021) +* Fixed angle convexity calculation for parallel and anti-parallel normals, + where a rounding error occasionally caused NaN angles in `getAngle3D()` + [[#1035]](https://github.com/PointCloudLibrary/pcl/pull/1035) +* Fixed undefined behavior when using multiple instances of `TimeTrigger` + [[#1074]](https://github.com/PointCloudLibrary/pcl/pull/1074) +* Fixed starvation bug in `TimeTrigger` on Windows with Boost < 1.55 + [[#1086]](https://github.com/PointCloudLibrary/pcl/pull/1086) +* Removed unnecessary mutex locking in `TimeTrigger::registerCallback` + [[#1087]](https://github.com/PointCloudLibrary/pcl/pull/1087) +* Updated PCL exception types to have nothrow copy constructor and copy + assigment operator + [[#1119]](https://github.com/PointCloudLibrary/pcl/pull/1119) +* Fixed a bug with `PCA` not triggering recomputation when input indices are + changed + [[#1167]](https://github.com/PointCloudLibrary/pcl/pull/1167) +* Added missing XYZ coordinate copying in `PointXYZRGBAtoXYZHSV` and + `PointXYZRGBtoXYZHSV` conversion functions + [[#1273]](https://github.com/PointCloudLibrary/pcl/pull/1273) +* Added `const` qualifiers where appropriate in point type conversion functions + [[#1274]](https://github.com/PointCloudLibrary/pcl/pull/1274) +* Fixed assignment operator in `PCA` + [[#1328]](https://github.com/PointCloudLibrary/pcl/pull/1328) +* Added `PointWithRange` to the list of core point types + [[#1352]](https://github.com/PointCloudLibrary/pcl/pull/1352) +* Fixed a bug in `getMaxDistance()` (this affected computation of OUR-CVFH + features) + [[#1449]](https://github.com/PointCloudLibrary/pcl/pull/1449) +* Added `operator==` to `PCLHeader` class + [[#1508]](https://github.com/PointCloudLibrary/pcl/pull/1508) + +### `libpcl_features:` + +* Fixed default parameters of the USC descriptor to match the values proposed in + the original paper + [[#856]](https://github.com/PointCloudLibrary/pcl/pull/856) +* Fixed the L1 normalization of the `ROPSEstimation` feature + [[#993]](https://github.com/PointCloudLibrary/pcl/pull/993) +* Fixed default angle step in `ROPSEstimation` + [[#1000]](https://github.com/PointCloudLibrary/pcl/pull/1000) +* Fixed a bug in `CRHEstimation` where internal spatial data vector was not + zero-initialized + [[#1151]](https://github.com/PointCloudLibrary/pcl/pull/1151) +* Updated `NormalEstimation` to mark cloud as non-dense when normal computation + fails + [[#1239]](https://github.com/PointCloudLibrary/pcl/pull/1239) +* Added new functions to compute approximate surface normals on a mesh and + approximate covariance matrices + [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262) +* Fixed histogram computation in `computePointPFHRGBSignature()` + [[#1331]](https://github.com/PointCloudLibrary/pcl/pull/1331) +* Fixed wrong erasing order in feature cache in `PFHEstimation` + [[#1335]](https://github.com/PointCloudLibrary/pcl/pull/1335) + +### `libpcl_filters:` + +* Improved `RadiusOutlierRemoval` performance by using nearest-K search when the + input point cloud is dense + [[#709]](https://github.com/PointCloudLibrary/pcl/pull/709) +* Fixed the signature of `BoxClipper3D::clipPlanarPolygon3D()` + [[#911]](https://github.com/PointCloudLibrary/pcl/pull/911) +* Updated base `Filter` class to allow using same point cloud as input and + output (effective for every filtering algorithm) + [[#1042]](https://github.com/PointCloudLibrary/pcl/pull/1042) +* Improved `CropBox` performance by caching the result of transform matrix + identity test + [[#1210]](https://github.com/PointCloudLibrary/pcl/pull/1210) +* Updated `PassThrough` filter to write a user-supplied value in place of bad + points + [[#1290]](https://github.com/PointCloudLibrary/pcl/pull/1290) +* Fixed handling of color fields in `VoxelGrid` centroid computation + [[#1415]](https://github.com/PointCloudLibrary/pcl/pull/1415) +* Updated `ExtractIndices` (for `PCLPointCloud2` cloud type) to respect + `keep_organized_` flag + [[#1462]](https://github.com/PointCloudLibrary/pcl/pull/1462) +* Fixed OpenMP support on MSVC in `Convolution3D` + [[#1527]](https://github.com/PointCloudLibrary/pcl/pull/1527) +* BugFix: Filters used applyFilter twice. + [[#1572]](https://github.com/PointCloudLibrary/pcl/pull/1572) + +### `libpcl_gpu:` + +* Added a function `hasShifted()` in KinFu large scale + [[#944]](https://github.com/PointCloudLibrary/pcl/pull/944) +* Fixed empty "View3D" window bug when using registration mode with `-pcd` flag + in KinFu app + [[#1018]](https://github.com/PointCloudLibrary/pcl/pull/1018) +* Fixed uninitialized loop variable in `PeoplePCDApp::convertProbToRGB()` + [[#1104]](https://github.com/PointCloudLibrary/pcl/pull/1104) +* Fixed compilation errors in `gpu_people` + [[#1194]](https://github.com/PointCloudLibrary/pcl/pull/1194) +* Fixed compilation error in `kinfu_large_scale` with CUDA ≥ 6.0 + [[#1225]](https://github.com/PointCloudLibrary/pcl/pull/1225) +* Fixed volume size computation in `kinfu_large_scale` + [[#1233]](https://github.com/PointCloudLibrary/pcl/pull/1233) +* Fixed sporadical out-of-bounds memory accesses in `kinfu_large_scale` kernels + [[#1263]](https://github.com/PointCloudLibrary/pcl/pull/1263) +* Fixed `plot_camera_poses.m` script in KinFu project + [[#1311]](https://github.com/PointCloudLibrary/pcl/pull/1311) +* Fixed runtime exceptions related to `--viz` flag in KinFu +* Fix compilation on Mac OSX + [[#1586]](https://github.com/PointCloudLibrary/pcl/pull/1586) + +### `libpcl_io:` + +* Added a grabber for IDS Imaging Ensenso devices + [[#888]](https://github.com/PointCloudLibrary/pcl/pull/888) +* Updated `RobotEyeGrabber` class to handle new packet format + [[#982]](https://github.com/PointCloudLibrary/pcl/pull/982) +* Fixed focal length computation in `OpenNI2Grabber` + [[#992]](https://github.com/PointCloudLibrary/pcl/pull/992) +* Updated `OpenNIGrabber` to use depth camera parameters instead of color camera + parameters for point reprojection + [[#994]](https://github.com/PointCloudLibrary/pcl/pull/994) +* Made PCD reader case insensitive with respect to nan/NaN/NAN values + [[#1004]](https://github.com/PointCloudLibrary/pcl/pull/1004) +* Added support for saving normal and curvature fields in `savePLYFile` and + `savePLYFileBinary` + [[#1057]](https://github.com/PointCloudLibrary/pcl/pull/1057) + [[#1058]](https://github.com/PointCloudLibrary/pcl/pull/1058) +* Fixed alpha value of bad points in `OpenNIGrabber` + [[#1090]](https://github.com/PointCloudLibrary/pcl/pull/1090) +* Fixed a bug in `OpenNIGrabber` destructor where wrong callback handle was + unregistered + [[#1094]](https://github.com/PointCloudLibrary/pcl/pull/1094) +* Fixed a bug in `PCDGrabber` destructor + [[#1127]](https://github.com/PointCloudLibrary/pcl/pull/1127) +* Fixed point coordinate computation in `HDLGrabber` + [[#1130]](https://github.com/PointCloudLibrary/pcl/pull/1130) +* Improved the PLY parser to work around some issues on Mac OSX + [[#1165]](https://github.com/PointCloudLibrary/pcl/pull/1165) +* Added a family of data buffer classes useful for temporal filtering in + grabbers + [[#1212]](https://github.com/PointCloudLibrary/pcl/pull/1212) +* Added a grabber for davidSDK devices + [[#1216]](https://github.com/PointCloudLibrary/pcl/pull/1216) +* Added a grabber and viewer for DepthSense SDK devices + [[#1230]](https://github.com/PointCloudLibrary/pcl/pull/1230) +* Fixed stride computation and alpha values in + `OpenNI2Grabber::convertToXYZRGBPointCloud()` + [[#1248]](https://github.com/PointCloudLibrary/pcl/pull/1248) +* Changed type and semantics of return values in polygon saving functions based + on VTK + [[#1279]](https://github.com/PointCloudLibrary/pcl/pull/1279) +* Moved implementations of `pcl::io::load()` and `pcl::io::save()` to a new file + "io/auto_io.h" + [[#1294]](https://github.com/PointCloudLibrary/pcl/pull/1294) +* Fixed compilation of `OpenNI2Grabber` on _msvc14_ + [[#1310]](https://github.com/PointCloudLibrary/pcl/pull/1310) +* Added a callback signal for the filename of grabbed PCD file in `PCDGrabber` + [[#1354]](https://github.com/PointCloudLibrary/pcl/pull/1354) +* Added support for both 'CRLF' and 'LF' line endings in PLY reader + [[#1370]](https://github.com/PointCloudLibrary/pcl/pull/1370) +* Updated OpenNI2 grabber to support devices without color stream + [[#1372]](https://github.com/PointCloudLibrary/pcl/pull/1372) +* Updated `PCDWriter` to not fail when the filesystem does not support setting + file permissions + [[#1374]](https://github.com/PointCloudLibrary/pcl/pull/1374) +* Fixed a bug in `MTLReader` reading function + [[#1380]](https://github.com/PointCloudLibrary/pcl/pull/1380) +* Removed `PXCGrabber` (superseded by `DepthSenseGrabber`) + [[#1395]](https://github.com/PointCloudLibrary/pcl/pull/1395) +* Added a grabber and viewer for RealSense SDK devices + [[#1401]](https://github.com/PointCloudLibrary/pcl/pull/1401) +* Updated `loadPLYFile()` to support NaN values + [[#1433]](https://github.com/PointCloudLibrary/pcl/pull/1433) +* Fixed parsing of `char` and `uchar` scalars in PLY files + [[#1443]](https://github.com/PointCloudLibrary/pcl/pull/1443) +* Fixed ASCII file support in `savePolygonFile*` functions + [[#1445]](https://github.com/PointCloudLibrary/pcl/pull/1445) +* Added a grabber and viewer for Velodyne VLP + [[#1452]](https://github.com/PointCloudLibrary/pcl/pull/1452) +* Fix compilation when WITH_VTK=OFF + [[#1585]](https://github.com/PointCloudLibrary/pcl/pull/1585) + +### `libpcl_keypoints:` + +* Fixed invalid array allocation in `ISSKeypoint3D` + [[#1022]](https://github.com/PointCloudLibrary/pcl/pull/1022) +* Removed superfluous parameter in 'TrajkovicKeypoint3D::getNormals()' + [[#1096]](https://github.com/PointCloudLibrary/pcl/pull/1096) +* Moved `UniformSampling` to the `filters` module + [[#1411]](https://github.com/PointCloudLibrary/pcl/pull/1411) +* Fixed OpenMP support in `HarrisKeypoint2D` + [[#1501]](https://github.com/PointCloudLibrary/pcl/pull/1501) +* Updated `SIFTKeypoint` to preserve point cloud viewpoint + [[#1508]](https://github.com/PointCloudLibrary/pcl/pull/1508) + +### `libpcl_octree:` + +* Added `const` qualifiers in `OctreePointCloud::getVoxelBounds()` + [[#1016]](https://github.com/PointCloudLibrary/pcl/pull/1016) +* Updated `Octree` iterator to use `unsigned long`s in key computations to + reduce chance of overflows + [[#1297]](https://github.com/PointCloudLibrary/pcl/pull/1297) +* Fixed compilation of `OctreePointCloudOccupancy` on _gcc_ + [[#1461]](https://github.com/PointCloudLibrary/pcl/pull/1461) + +### `libpcl_outofcore:` + +* Fixed compilation errors with C++11 standard + [[#1386]](https://github.com/PointCloudLibrary/pcl/pull/1386) + +### `libpcl_people:` + +* Fixed undefined behavior in `HOG` (use `new`/`delete` consistently) + [[#1099]](https://github.com/PointCloudLibrary/pcl/pull/1099) + +### `libpcl_recognition:` + +* Fixed multiple includes in `recognition` module + [[#1109]](https://github.com/PointCloudLibrary/pcl/pull/1109) + [[#1110]](https://github.com/PointCloudLibrary/pcl/pull/1110) +* Fixed "index out of bounds" error in `LineRGBD::refineDetectionsAlongDepth()` + [[#1117]](https://github.com/PointCloudLibrary/pcl/pull/1117) +* Fixed a memory leak in `LINEMOD::detectTemplatesSemiScaleInvariant()` + [[#1184]](https://github.com/PointCloudLibrary/pcl/pull/1184) + +### `libpcl_registration:` + +* Updated `GeneralizedIterativeClosestPoint` to return _transformed_ input point + cloud after alignment + [[#887]](https://github.com/PointCloudLibrary/pcl/pull/887) +* Fixed a problem with multiple definition of `setInputFeatureCloud` and + `nearestNeighborSearch` symbols in `PPFRegistration` + [[#891]](https://github.com/PointCloudLibrary/pcl/pull/891) + [[#907]](https://github.com/PointCloudLibrary/pcl/pull/907) +* Added an implementation of the algorithm "4-Points Congruent Sets for Robust + Surface Registration" + [[#976]](https://github.com/PointCloudLibrary/pcl/pull/976) +* Added an implementation of the algorithm "Keypoint-based 4-Points Congruent + Sets – Automated marker-less registration of laser scans" + [[#979]](https://github.com/PointCloudLibrary/pcl/pull/979) +* Fixed compilation of `pcl_registration` module with MSVC2010 + [[#1014]](https://github.com/PointCloudLibrary/pcl/pull/1014) +* Removed wrong error normalization in `SampleConsensusPrerejective` + [[#1037]](https://github.com/PointCloudLibrary/pcl/pull/1037) +* Added a new `IncrementalRegistration` class that allows to register a stream + of clouds where each cloud is aligned to the previous cloud + [[#1202]](https://github.com/PointCloudLibrary/pcl/pull/1202) + [[#1451]](https://github.com/PointCloudLibrary/pcl/pull/1451) +* Fixed a wrong typedef for `KdTreeReciprocalPtr` + [[#1204]](https://github.com/PointCloudLibrary/pcl/pull/1204) +* Added support for externally computed covariance matrices in + `GeneralizedIterativeClosestPoint` + [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262) +* Fixed initialization of source and target covariances in + `GeneralizedIterativeClosestPoint6D` + [[#1304]](https://github.com/PointCloudLibrary/pcl/pull/1304) +* Added a new `MetaRegistration` class that allows to register a stream of + clouds where each cloud is aligned to the conglomerate of all previous clouds + [[#1426]](https://github.com/PointCloudLibrary/pcl/pull/1426) +* Fixed segmentation fault occuring in `CorrespondenceRejectorSurfaceNormal` + [[#1536]](https://github.com/PointCloudLibrary/pcl/pull/1536) +* Use aligned allocator in vectors of MatchingCandidate + [[#1552]](https://github.com/PointCloudLibrary/pcl/pull/1552) + +### `libpcl_sample_consensus:` + +* Fixed behavior of `SACMODEL_PARALLEL_LINE` to match the name (instead of + searching for lines perpendicular to a given axis) + [[#969]](https://github.com/PointCloudLibrary/pcl/pull/969) +* Added `getClassName()` function to all SAC models + [[#1071]](https://github.com/PointCloudLibrary/pcl/pull/1071) +* Improved performance of `SampleConsensusModel::computeVariance()` by up to 10 + times + [[#1285]](https://github.com/PointCloudLibrary/pcl/pull/1285) +* Fixed assignment operators for `SacModelCone` and `SacModelCylinder` + [[#1299]](https://github.com/PointCloudLibrary/pcl/pull/1299) +* Refactored SAC models to store expected model and sample sizes in a protected + member field; this deprecated `SAC_SAMPLE_SIZE` map + [[#1367]](https://github.com/PointCloudLibrary/pcl/pull/1367) + [[#1396]](https://github.com/PointCloudLibrary/pcl/pull/1396) + +### `libpcl_search:` + +* Fixed potential segfault in `OrganizedNeighbor::estimateProjectionMatrix()` + [[#1176]](https://github.com/PointCloudLibrary/pcl/pull/1176) + +### `libpcl_segmentation:` + +* Added implementation of `LCCP` segmentation algorithm + [[#718]](https://github.com/PointCloudLibrary/pcl/pull/718) + [[#1287]](https://github.com/PointCloudLibrary/pcl/pull/1287) + [[#1389]](https://github.com/PointCloudLibrary/pcl/pull/1389) +* Made `SupervoxelClustering` fully deterministic and made some internal + refactoring + [[#912]](https://github.com/PointCloudLibrary/pcl/pull/912) +* Moved specializations of `OctreePointCloudAdjacency::VoxelData` class from + header to implementation files + [[#919]](https://github.com/PointCloudLibrary/pcl/pull/919) +* Deprecated `SupervoxelClustering::getColoredCloud()` + [[#941]](https://github.com/PointCloudLibrary/pcl/pull/941) +* Fixed a regression in `ExtractPolygonalPrismData`; both explicitly and + implicitly closed polygons are supported again + [[#1044]](https://github.com/PointCloudLibrary/pcl/pull/1044) +* Added an overload of `setConditionFunction()` in + `ConditionalEuclideanClustering` that takes `boost::function` + [[#1050]](https://github.com/PointCloudLibrary/pcl/pull/1050) +* Updated `SupervoxelClustering` to use the depth dependent transform by + default only if the input cloud is organized; added a function to force use + of the transform, and removed corresponding parameter from the constructor + [[#1115]](https://github.com/PointCloudLibrary/pcl/pull/1115) +* Substituted hard-coded label point type with template parameter in + `OrganizedConnectedComponentSegmentation` + [[#1264]](https://github.com/PointCloudLibrary/pcl/pull/1264) +* Added an implementation of supervoxel graph partitioning algorithm described + in "Constrained Planar Cuts - Object Partitioning for Point Clouds" + [[#1278]](https://github.com/PointCloudLibrary/pcl/pull/1278) +* Fixed crashes in `ApproximateProgressiveMorphologicalFilter` in the case of + non-default cell size + [[#1293]](https://github.com/PointCloudLibrary/pcl/pull/1293) +* Fixed a bug in `RegionGrowing::validatePoint()` + [[#1327]](https://github.com/PointCloudLibrary/pcl/pull/1327) +* Fixed return value of `SupervoxelClustering::getSeedResolution()` + [[#1339]](https://github.com/PointCloudLibrary/pcl/pull/1339) + +### `libpcl_stereo:` + +* Added a new `DisparityMapConverter` class for converting disparity maps into + point clouds + [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021) +* Added a new `DigitalElevationMapBuilder` class for building Digital Elevation + Maps from disparity maps + [[#1021]](https://github.com/PointCloudLibrary/pcl/pull/1021) + +### `libpcl_surface:` + +* Updated `TextureMapping` to not use hard-coded point types + [[#929]](https://github.com/PointCloudLibrary/pcl/pull/929) +* Added a new function `getHullPointIndices` in concave and convex hull classes + to retrieve indices of points that form the computed hull + [[#1213]](https://github.com/PointCloudLibrary/pcl/pull/1213) +* Added several functions and parameters to the `OrganizedFastMesh` class + [[#1262]](https://github.com/PointCloudLibrary/pcl/pull/1262) +* Added missing `PCL_EXPORTS` attributes for OpenNURBS classes + [[#1315]](https://github.com/PointCloudLibrary/pcl/pull/1315) +* Fixed memory leak in `MeshSmoothingLaplacianVTK` + [[#1424]](https://github.com/PointCloudLibrary/pcl/pull/1424) + +### `libpcl_tracking:` + +* Improved OMP 2.0 compatibility of `PyramidalKLTTracker` + [[#1214]](https://github.com/PointCloudLibrary/pcl/pull/1214) + [[#1223]](https://github.com/PointCloudLibrary/pcl/pull/1223) +* Fixed occasional segfault in `KLDAdaptiveParticleFilterOMPTracker` + [[#1392]](https://github.com/PointCloudLibrary/pcl/pull/1392) + +### `libpcl_visualization:` + +* Added a new `PointCloudColorHandler` for "label" field + [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849) +* Fixed `setSize()` and `setPosition()` functions in `PCLVisualizer` + [[#923]](https://github.com/PointCloudLibrary/pcl/pull/923) +* Fixed an issue with `PCLVisualizer` producing empty screenshots on some system + configurations + [[#956]](https://github.com/PointCloudLibrary/pcl/pull/956) +* Added a new function `removeAllCoordinateSystems()` in `PCLVisualizer` + [[#965]](https://github.com/PointCloudLibrary/pcl/pull/965) +* Made `PCLVisualizer::addPointCloudPrincipalCurvatures()` templated on point + and normal type + [[#965]](https://github.com/PointCloudLibrary/pcl/pull/965) +* Fixed a minor bug in `PCLVisualizer::updatePolygonMesh()` + [[#977]](https://github.com/PointCloudLibrary/pcl/pull/977) +* Fixed a minor bug in `ImageViewer::addMask()` + [[#990]](https://github.com/PointCloudLibrary/pcl/pull/990) +* Fixed opacity handling in `ImageViewer` + [[#995]](https://github.com/PointCloudLibrary/pcl/pull/995) +* Fixed a bug with `ImageViewer` not displaying anything with VTK 6 + [[#1009]](https://github.com/PointCloudLibrary/pcl/pull/1009) +* Updated `ImageViewer` to work around a bug in VTK 6.1 + [[#1017]](https://github.com/PointCloudLibrary/pcl/pull/1017) +* Fixed an Eigen-related compilation error in `PCLVisualizer::renderView()` + [[#1019]](https://github.com/PointCloudLibrary/pcl/pull/1019) +* Fixed wrong axis flipping in `PCLVisualizer::renderView()` + [[#1026]](https://github.com/PointCloudLibrary/pcl/pull/1026) +* Fixed a bug in `renderViewTesselatedSphere` when generated vertices were not + guaranteed to stay on the unit sphere + [[#1043]](https://github.com/PointCloudLibrary/pcl/pull/1043) +* Fixed misaligned context items in `ImageViewer` + [[#1049]](https://github.com/PointCloudLibrary/pcl/pull/1049) +* Fixed opacity handling for layered rectangles of context items in + `ImageViewer` + [[#1051]](https://github.com/PointCloudLibrary/pcl/pull/1051) +* Fixed a regression in `RenderViewsTesselatedSphere::generateViews()` related + to handling of multiple VTK versions + [[#1056]](https://github.com/PointCloudLibrary/pcl/pull/1056) + [[#1067]](https://github.com/PointCloudLibrary/pcl/pull/1067) + [[#1072]](https://github.com/PointCloudLibrary/pcl/pull/1072) +* Updated `PCLVisualizer` to use `PointCloudColorHandlerRGBAField` for + `PointXYZRGBA` clouds by default + [[#1064]](https://github.com/PointCloudLibrary/pcl/pull/1064) +* Fixed a bug in `PointCloudColorHandlerLabelField` where red and blue channels + were swapped + [[#1076]](https://github.com/PointCloudLibrary/pcl/pull/1076) +* Updated `PCLPlotter` to ignore NaN values in histogram computation + [[#1120]](https://github.com/PointCloudLibrary/pcl/pull/1120) + [[#1126]](https://github.com/PointCloudLibrary/pcl/pull/1126) +* Fixed initial size of the `PCLVisualizer` window + [[#1125]](https://github.com/PointCloudLibrary/pcl/pull/1125) +* Changed default representation of all shapes in `PCLVisualizer` to "surface" + [[#1132]](https://github.com/PointCloudLibrary/pcl/pull/1132) +* Added a check for model coefficients size in functions that add shapes to + `PCLVisualizer` + [[#1142]](https://github.com/PointCloudLibrary/pcl/pull/1142) +* Added an option to switch between static/optimal color assignment in + `PointCloudColorHandlerLabelField` + [[#1156]](https://github.com/PointCloudLibrary/pcl/pull/1156) +* Added `PCLVisualizer::contains()` to check if a cloud, shape, or coordinate + axes with given id already exist + [[#1181]](https://github.com/PointCloudLibrary/pcl/pull/1181) +* Improved shape visualization by enabling shading + [[#1211]](https://github.com/PointCloudLibrary/pcl/pull/1211) +* Improved 'u' key functionality in `PCLVisualizer` + [[#1241]](https://github.com/PointCloudLibrary/pcl/pull/1241) + [[#1321]](https://github.com/PointCloudLibrary/pcl/pull/1321) + [[#1323]](https://github.com/PointCloudLibrary/pcl/pull/1323) +* Fixed potential crashes in `PCLVisualizer` by always checking result of + `vtkSafeDownCast` calls + [[#1245]](https://github.com/PointCloudLibrary/pcl/pull/1245) +* Updated `addPointCloud()` to use `PointCloudColorHandlerRGBField` when the + cloud has color field + [[#1295]](https://github.com/PointCloudLibrary/pcl/pull/1295) + [[#1325]](https://github.com/PointCloudLibrary/pcl/pull/1325) +* Updated `PCLVisualizer` not to disable shading when changing shape's color + [[#1300]](https://github.com/PointCloudLibrary/pcl/pull/1300) +* Fixed behavior of `PCLVisualizer::wasStopped()` with VTK6 on OSX + [[#1436]](https://github.com/PointCloudLibrary/pcl/pull/1436) +* Improve pointcloud visualization with colormaps + [[#1581]](https://github.com/PointCloudLibrary/pcl/pull/1581) + +### `PCL Apps:` + +* Fixed compilation of `point_cloud_editor` with Qt5 + [[#935]](https://github.com/PointCloudLibrary/pcl/pull/935) +* Fixed compilation of `dominant_plane_segmentation` and `manual_registration` + with Boost 1.57 + [[#1062]](https://github.com/PointCloudLibrary/pcl/pull/1062) + [[#1063]](https://github.com/PointCloudLibrary/pcl/pull/1063) + +### `PCL Examples:` + +* Updated supervoxel clustering example + [[#915]](https://github.com/PointCloudLibrary/pcl/pull/915) +* Fixes for MS Visual Studio 2013 + [[#1526]](https://github.com/PointCloudLibrary/pcl/pull/1526) + +### `PCL Tools:` + +* Added support for point label visualization in `pcl_viewer` + [[#849]](https://github.com/PointCloudLibrary/pcl/pull/849) +* Added support for absolute positioning of visualized point clouds in + `pcl_viewer` + [[#1154]](https://github.com/PointCloudLibrary/pcl/pull/1154) +* Fixed PLY file loading in `pcl_mesh_sampling` tool + [[#1155]](https://github.com/PointCloudLibrary/pcl/pull/1155) +* Added loop distance (`-D`) and loop count (`-c`) parameters to the LUM tool + [[#1291]](https://github.com/PointCloudLibrary/pcl/pull/1291) +* Fixed in-place filtering with `VoxelGrid` in `mesh_sampling` tool + [[#1366]](https://github.com/PointCloudLibrary/pcl/pull/1366) +* Added a tool to convert OBJ files to PLY format + [[#1375]](https://github.com/PointCloudLibrary/pcl/pull/1375) +* Added a universal mesh/cloud converted tool to convert between OBJ, PCD, PLY, + STL, and VTK files + [[#1442]](https://github.com/PointCloudLibrary/pcl/pull/1442) + ## *= 1.7.2 (10.09.2014) =* * Added support for VTK6 @@ -57,6 +568,8 @@ [[#811]](https://github.com/PointCloudLibrary/pcl/pull/811) * Fixed memory corruption error in OUR-CVFH [[#875]](https://github.com/PointCloudLibrary/pcl/pull/875) +* Declare `const InterestPoint&` explicitly + [[#1541]](https://github.com/PointCloudLibrary/pcl/pull/1541) ### `libpcl_filters:` @@ -196,6 +709,8 @@ [[#689]](https://github.com/PointCloudLibrary/pcl/pull/689) * Reduced space usage in `MovingLeastSquares` [[#785]](https://github.com/PointCloudLibrary/pcl/pull/785) +* Adds MLS instantiation for input type PointXYZRGBNormal + [[#1545]](https://github.com/PointCloudLibrary/pcl/pull/1545) ### `libpcl_tracking:` diff --git a/CMakeLists.txt b/CMakeLists.txt index f0a56003..e5fd763e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,10 +1,24 @@ ### ---[ PCL global CMake cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +if(POLICY CMP0017) + # Do not include files in CMAKE_MODULE_PATH from files + # in CMake module directory. Fix MXE build + cmake_policy(SET CMP0017 NEW) +endif() + +if(POLICY CMP0020 AND (WIN32 AND NOT MINGW)) + cmake_policy(SET CMP0020 NEW) # Automatically link Qt executables to qtmain target on Windows +endif() + if(POLICY CMP0048) cmake_policy(SET CMP0048 OLD) # do not use VERSION option in project() command endif() +if(POLICY CMP0054) + cmake_policy(SET CMP0054 OLD) # Silent warnings about quoted variables +endif() + set(CMAKE_CONFIGURATION_TYPES "Debug;Release" CACHE STRING "possible configurations" FORCE) # In case the user does not setup CMAKE_BUILD_TYPE, assume it's RelWithDebInfo @@ -65,10 +79,15 @@ SET(CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel Maintainer." FORCE) -# ---[ Android check -if (ANDROID) - set (PCL_SHARED_LIBS OFF) - message ("PCL shared libs on Android must be: ${PCL_SHARED_LIBS}") +# Compiler identification +# Define a variable CMAKE_COMPILER_IS_X where X is the compiler short name. +# Note: CMake automatically defines one for GNUCXX, nothing to do in this case. +if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") + set(CMAKE_COMPILER_IS_CLANG 1) +elseif(__COMPILER_PATHSCALE) + set(CMAKE_COMPILER_IS_PATHSCALE 1) +elseif(MSVC) + set(CMAKE_COMPILER_IS_MSVC 1) endif() include("${PCL_SOURCE_DIR}/cmake/pcl_verbosity.cmake") @@ -108,11 +127,16 @@ if(CMAKE_COMPILER_IS_GNUCXX) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") endif(NOT ANDROID) + if("${CMAKE_SHARED_LINKER_FLAGS}" STREQUAL "" AND NOT CMAKE_SYSTEM_NAME STREQUAL "Darwin") + SET(CMAKE_SHARED_LINKER_FLAGS "-Wl,--as-needed") + endif() + if(WIN32) if(PCL_SHARED_LIBS) SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--export-all-symbols -Wl,--enable-auto-import") if (MINGW) add_definitions("-DBOOST_THREAD_USE_LIB") + SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--allow-multiple-definition") endif() else(PCL_SHARED_LIBS) add_definitions("-DBOOST_LIB_DIAGNOSTIC -DBOOST_THREAD_USE_LIB") @@ -120,16 +144,15 @@ if(CMAKE_COMPILER_IS_GNUCXX) endif() endif() -if(MSVC) - SET(CMAKE_COMPILER_IS_MSVC 1) - add_definitions ("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX /bigobj") +if(CMAKE_COMPILER_IS_MSVC) + add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES /bigobj ${SSE_DEFINITIONS}") if("${CMAKE_CXX_FLAGS}" STREQUAL " /DWIN32 /D_WINDOWS /W3 /GR /EHsc") # Check against default flags SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj /EHsc /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS}") # Add extra code generation/link optimizations if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION) SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /GL") - SET(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG") + SET(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /LTCG /OPT:REF") SET(CMAKE_EXE_LINKER_FLAGS_RELEASE "${CMAKE_EXE_LINKER_FLAGS_RELEASE} /LTCG") endif(CMAKE_MSVC_CODE_LINK_OPTIMIZATION) # /MANIFEST:NO") # please, don't disable manifest generation, otherwise crash at start for vs2008 @@ -145,15 +168,16 @@ if(MSVC) endif() endif() -if (__COMPILER_PATHSCALE) - SET(CMAKE_COMPILER_IS_PATHSCALE 1) +if(CMAKE_COMPILER_IS_PATHSCALE) if("${CMAKE_CXX_FLAGS}" STREQUAL "") SET(CMAKE_CXX_FLAGS "-Wno-uninitialized -zerouv -pthread -mp") endif() + if("${CMAKE_SHARED_LINKER_FLAGS}" STREQUAL "") + SET(CMAKE_SHARED_LINKER_FLAGS "-mp") + endif() endif() -if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - SET(CMAKE_COMPILER_IS_CLANG 1) +if(CMAKE_COMPILER_IS_CLANG) if("${CMAKE_C_FLAGS}" STREQUAL "") SET(CMAKE_C_FLAGS "-Qunused-arguments") endif() @@ -167,7 +191,7 @@ if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang") endif() include("${PCL_SOURCE_DIR}/cmake/pcl_utils.cmake") -set(PCL_VERSION 1.7.2 CACHE STRING "PCL version") +set(PCL_VERSION 1.8.0 CACHE STRING "PCL version") DISSECT_VERSION() GET_OS_INFO() SET_INSTALL_DIRS() @@ -216,27 +240,36 @@ ENDIF("${is_system_dir}" STREQUAL "-1") ### ---[ Find universal dependencies # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it -if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) +if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_COMPILER_IS_CLANG)) find_package(OpenMP) endif() if(OPENMP_FOUND) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") message (STATUS "Found OpenMP") - if(MSVC90 OR MSVC10) + if(MSVC) if(MSVC90) set(OPENMP_DLL VCOMP90) elseif(MSVC10) set(OPENMP_DLL VCOMP100) + elseif(MSVC11) + set(OPENMP_DLL VCOMP110) + elseif(MSVC12) + set(OPENMP_DLL VCOMP120) + elseif(MSVC14) + set(OPENMP_DLL VCOMP140) endif(MSVC90) - set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll") - set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll") - endif(MSVC90 OR MSVC10) + if(OPENMP_DLL) + set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll") + set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll") + else(OPENMP_DLL) + message(WARNING "Delay loading flag for OpenMP DLL is invalid.") + endif(OPENMP_DLL) + endif(MSVC) else(OPENMP_FOUND) message (STATUS "Not found OpenMP") endif() -# Boost (required) -include("${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake") + # Eigen (required) find_package(Eigen REQUIRED) include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) @@ -258,45 +291,14 @@ if(WITH_LIBUSB) endif(LIBUSB_1_FOUND) endif(WITH_LIBUSB) -# OpenNI -option(WITH_OPENNI "OpenNI driver support" TRUE) -if(WITH_OPENNI) - find_package(OpenNI) - if (OPENNI_FOUND) - set(HAVE_OPENNI ON) - include_directories(SYSTEM ${OPENNI_INCLUDE_DIRS}) - endif(OPENNI_FOUND) -endif(WITH_OPENNI) - -# OpenNI 2 -option(WITH_OPENNI2 "OpenNI 2 driver support" TRUE) -if(WITH_OPENNI2) - find_package(OpenNI2) - if (OPENNI2_FOUND) - set(HAVE_OPENNI2 ON) - include_directories(SYSTEM ${OPENNI2_INCLUDE_DIRS}) - endif(OPENNI2_FOUND) -endif(WITH_OPENNI2) - -# Fotonic (FZ_API) -option(WITH_FZAPI "Build Fotonic Camera support" TRUE) -if(WITH_FZAPI) - find_package(FZAPI) - if (FZAPI_FOUND) - set(HAVE_FZAPI ON) - include_directories(SYSTEM "${FZAPI_INCLUDE_DIR}") - endif(FZAPI_FOUND) -endif(WITH_FZAPI) - -# Intel Perceptional Computing Interface (PXCAPI) -option(WITH_PXCAPI "Build PXC Device support" TRUE) -if(WITH_PXCAPI) - find_package(PXCAPI) - if (PXCAPI_FOUND) - set(HAVE_PXCAPI ON) - include_directories(SYSTEM ${PXCAPI_INCLUDE_DIRS}) - endif(PXCAPI_FOUND) -endif(WITH_PXCAPI) +# Dependencies for different grabbers +PCL_ADD_GRABBER_DEPENDENCY("OpenNI" "OpenNI grabber support") +PCL_ADD_GRABBER_DEPENDENCY("OpenNI2" "OpenNI2 grabber support") +PCL_ADD_GRABBER_DEPENDENCY("FZAPI" "Fotonic camera support") +PCL_ADD_GRABBER_DEPENDENCY("Ensenso" "IDS-Imaging Ensenso camera support") +PCL_ADD_GRABBER_DEPENDENCY("davidSDK" "David Vision Systems SDK support") +PCL_ADD_GRABBER_DEPENDENCY("DSSDK" "DepthSense SDK support") +PCL_ADD_GRABBER_DEPENDENCY("RSSDK" "RealSense SDK support") # metslib if (PKG_CONFIG_FOUND) @@ -328,20 +330,30 @@ if(WITH_QHULL) set(QHULL_USE_STATIC ON) endif(NOT PCL_SHARED_LIBS OR WIN32) find_package(Qhull) + if(QHULL_FOUND) + include_directories(${QHULL_INCLUDE_DIRS}) + endif(QHULL_FOUND) endif(WITH_QHULL) +# Cuda +option(WITH_CUDA "Build NVIDIA-CUDA support" TRUE) +if(WITH_CUDA) + include("${PCL_SOURCE_DIR}/cmake/pcl_find_cuda.cmake") +endif(WITH_CUDA) + option(WITH_QT "Build QT Front-End" TRUE) if(WITH_QT) - # Find Qt4 - find_package(Qt4) - if (QT4_FOUND) - include("${QT_USE_FILE}") - endif (QT4_FOUND) - - # Find QT5 - if(NOT QT4_FOUND) - include(cmake/pcl_find_qt5.cmake) - endif(NOT QT4_FOUND) + set(PCL_QT_VERSION 5 CACHE STRING "Which QT version to use") + if("${PCL_QT_VERSION}" STREQUAL "4") + find_package(Qt4) + if (QT4_FOUND) + include("${QT_USE_FILE}") + endif (QT4_FOUND) + elseif("${PCL_QT_VERSION}" STREQUAL "5") + include(cmake/pcl_find_qt5.cmake) + else() + message(SEND_ERROR "PCL_QT_VERSION must be 4 or 5") + endif() endif(WITH_QT) # Find VTK @@ -349,7 +361,12 @@ option(WITH_VTK "Build VTK-Visualizations" TRUE) if(WITH_VTK AND NOT ANDROID) find_package(VTK) if(VTK_FOUND) - message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}") + if(NOT DEFINED VTK_RENDERING_BACKEND) + # On old VTK versions this variable does not exist. In this case it is + # safe to assume OpenGL backend + set(VTK_RENDERING_BACKEND "OpenGL") + endif() + message(STATUS "VTK_MAJOR_VERSION ${VTK_MAJOR_VERSION}, rendering backend: ${VTK_RENDERING_BACKEND}") if (PCL_SHARED_LIBS OR (NOT (PCL_SHARED_LIBS) AND NOT (VTK_BUILD_SHARED_LIBS))) set(VTK_FOUND TRUE) @@ -365,6 +382,11 @@ if(WITH_VTK AND NOT ANDROID) option (VTK_USE_COCOA "Use Cocoa for VTK render windows" ON) MARK_AS_ADVANCED (VTK_USE_COCOA) endif (APPLE) + if(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL") + set(VTK_RENDERING_BACKEND_OPENGL_VERSION "1") + elseif(${VTK_RENDERING_BACKEND} STREQUAL "OpenGL2") + set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2") + endif() set(HAVE_VTK ON) else () set(VTK_FOUND OFF) @@ -386,7 +408,13 @@ if(WITH_PCAP) endif(WITH_PCAP) # OpenGL and GLUT -include("${PCL_SOURCE_DIR}/cmake/pcl_find_gl.cmake") +option(WITH_OPENGL "Support for OpenGL" TRUE) +if(WITH_OPENGL) + include("${PCL_SOURCE_DIR}/cmake/pcl_find_gl.cmake") +endif(WITH_OPENGL) + +# Boost (required) +include("${PCL_SOURCE_DIR}/cmake/pcl_find_boost.cmake") ### ---[ Create the config.h file set(pcl_config_h_in "${CMAKE_CURRENT_SOURCE_DIR}/pcl_config.h.in") @@ -435,3 +463,4 @@ MAKE_DEP_GRAPH() ### ---[ Finish up PCL_WRITE_STATUS_REPORT() PCL_RESET_MAPS() + diff --git a/PCLConfig.cmake.in b/PCLConfig.cmake.in index 65ea6881..994d20e5 100644 --- a/PCLConfig.cmake.in +++ b/PCLConfig.cmake.in @@ -174,15 +174,15 @@ macro(find_qhull) endif(QHULL_FOUND) endmacro(find_qhull) -#remove this as soon as openni-dev is shipped with FindOpenni.cmake +#remove this as soon as libopenni is shipped with FindOpenni.cmake macro(find_openni) - if(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "ON")) + if(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "TRUE")) set(OPENNI_INCLUDE_DIRS_HINT "@OPENNI_INCLUDE_DIRS@") get_filename_component(OPENNI_LIBRARY_HINT "@OPENNI_LIBRARY@" PATH) - endif(NOT OPENNI_ROOT AND ("@HAVE_OPENNI@" STREQUAL "ON")) + endif() if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_OPENNI openni-dev) + pkg_check_modules(PC_OPENNI libopenni) endif(PKG_CONFIG_FOUND) find_path(OPENNI_INCLUDE_DIRS XnStatus.h HINTS ${PC_OPENNI_INCLUDEDIR} ${PC_OPENNI_INCLUDE_DIRS} @@ -210,11 +210,16 @@ macro(find_openni) endif(OPENNI_FOUND) endmacro(find_openni) -#remove this as soon as openni2-dev is shipped with FindOpenni2.cmake +#remove this as soon as libopenni2 is shipped with FindOpenni2.cmake macro(find_openni2) - if(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) - get_filename_component(OPENNI2_LIBRARY_HINT "OPENNI_LIBRARY-NOTFOUND" PATH) - endif(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) + if(PCL_FIND_QUIETLY) + set(OpenNI2_FIND_QUIETLY TRUE) + endif() + + if(NOT OPENNI2_ROOT AND ("@HAVE_OPENNI2@" STREQUAL "TRUE")) + set(OPENNI2_INCLUDE_DIRS_HINT "@OPENNI2_INCLUDE_DIRS@") + get_filename_component(OPENNI2_LIBRARY_HINT "@OPENNI2_LIBRARY@" PATH) + endif() set(OPENNI2_SUFFIX) if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) @@ -222,31 +227,182 @@ macro(find_openni2) endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) if(PKG_CONFIG_FOUND) - pkg_check_modules(PC_OPENNI2 openni2-dev) + if(PCL_FIND_QUIETLY) + pkg_check_modules(PC_OPENNI2 QUIET libopenni2) + else() + pkg_check_modules(PC_OPENNI2 libopenni2) + endif() endif(PKG_CONFIG_FOUND) find_path(OPENNI2_INCLUDE_DIRS OpenNI.h - HINTS /usr/include/openni2 /usr/include/ni2 - PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" + HINTS /usr/include/openni2 /usr/include/ni2 /usr/local/include/openni2 /usr/local/include/ni2 + PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" "${OPENNI2_INCLUDE_DIRS_HINT}" PATH_SUFFIXES openni openni2 include Include) find_library(OPENNI2_LIBRARY NAMES OpenNI2 # No suffix needed on Win64 - HINTS /usr/lib - PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" + HINTS /usr/lib /usr/local/lib/ni2 + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" "${OPENNI2_LIBRARY_HINT}" PATH_SUFFIXES lib Lib Lib64) include(FindPackageHandleStandardArgs) find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) if(OPENNI2_FOUND) - get_filename_component(OPENNI_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH) + get_filename_component(OPENNI2_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH) set(OPENNI2_LIBRARY_DIRS ${OPENNI2_LIBRARY_PATH}) set(OPENNI2_LIBRARIES "${OPENNI2_LIBRARY}") set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) endif(OPENNI2_FOUND) endmacro(find_openni2) +#remove this as soon as the Ensenso SDK is shipped with FindEnsenso.cmake +macro(find_ensenso) + if(PCL_FIND_QUIETLY) + set(ensenso_FIND_QUIETLY TRUE) + endif() + + if(NOT ENSENSO_ROOT AND ("@HAVE_ENSENSO@" STREQUAL "TRUE")) + get_filename_component(ENSENSO_ABI_HINT @ENSENSO_INCLUDE_DIR@ PATH) + endif() + + find_path(ENSENSO_INCLUDE_DIR nxLib.h + HINTS ${ENSENSO_ABI_HINT} + /opt/ensenso/development/c + "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c" + PATH_SUFFIXES include/) + + find_library(ENSENSO_LIBRARY QUIET NAMES NxLib64 NxLib32 nxLib64 nxLib32 + HINTS ${ENSENSO_ABI_HINT} + "$ENV{PROGRAMFILES}/Ensenso/development/c" "$ENV{PROGRAMW6432}/Ensenso/development/c" + PATH_SUFFIXES lib/) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(ensenso DEFAULT_MSG + ENSENSO_LIBRARY ENSENSO_INCLUDE_DIR) + + if(ENSENSO_FOUND) + get_filename_component(ENSENSO_LIBRARY_PATH ${ENSENSO_LIBRARY} PATH) + set(ENSENSO_INCLUDE_DIRS ${ENSENSO_INCLUDE_DIR}) + set(ENSENSO_LIBRARY_DIRS ${ENSENSO_LIBRARY_PATH}) + set(ENSENSO_LIBRARIES "${ENSENSO_LIBRARY}") + set(ENSENSO_REDIST_DIR $ENV{ENSENSO_REDIST${ENSENSO_SUFFIX}}) + endif(ENSENSO_FOUND) +endmacro(find_ensenso) + +#remove this as soon as the davidSDK is shipped with FinddavidSDK.cmake +macro(find_davidSDK) + if(PCL_FIND_QUIETLY) + set(DAVIDSDK_FIND_QUIETLY TRUE) + endif() + + if(NOT davidSDK_ROOT AND ("@HAVE_DAVIDSDK@" STREQUAL "TRUE")) + get_filename_component(DAVIDSDK_ABI_HINT @DAVIDSDK_INCLUDE_DIR@ PATH) + endif() + + find_path(DAVIDSDK_INCLUDE_DIR david.h + HINTS ${DAVIDSDK_ABI_HINT} + /usr/local/include/davidSDK + "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK" + PATH_SUFFIXES include/) + + find_library(DAVIDSDK_LIBRARY QUIET NAMES davidSDK + HINTS ${DAVIDSDK_ABI_HINT} + "$ENV{PROGRAMFILES}/davidSDK" "$ENV{PROGRAMW6432}/davidSDK" + PATH_SUFFIXES lib/) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(DAVIDSDK DEFAULT_MSG + DAVIDSDK_LIBRARY DAVIDSDK_INCLUDE_DIR) + + if(DAVIDSDK_FOUND) + get_filename_component(DAVIDSDK_LIBRARY_PATH ${DAVIDSDK_LIBRARY} PATH) + set(DAVIDSDK_INCLUDE_DIRS ${DAVIDSDK_INCLUDE_DIR}) + set(DAVIDSDK_LIBRARY_DIRS ${DAVIDSDK_LIBRARY_PATH}) + set(DAVIDSDK_LIBRARIES "${DAVIDSDK_LIBRARY}") + set(DAVIDSDK_REDIST_DIR $ENV{DAVIDSDK_REDIST${DAVIDSDK_SUFFIX}}) + endif(DAVIDSDK_FOUND) +endmacro(find_davidSDK) + +macro(find_dssdk) + if(PCL_FIND_QUIETLY) + set(DSSDK_FIND_QUIETLY TRUE) + endif() + if(NOT DSSDK_DIR AND ("@HAVE_DSSDK@" STREQUAL "TRUE")) + get_filename_component(DSSDK_DIR_HINT "@DSSDK_INCLUDE_DIRS@" PATH) + endif() + find_path(DSSDK_DIR include/DepthSenseVersion.hxx + HINTS ${DSSDK_DIR_HINT} + "$ENV{DEPTHSENSESDK32}" + "$ENV{DEPTHSENSESDK64}" + PATHS "$ENV{PROGRAMFILES}/SoftKinetic/DepthSenseSDK" + "$ENV{PROGRAMW6432}/SoftKinetic/DepthSenseSDK" + "C:/Program Files (x86)/SoftKinetic/DepthSenseSDK" + "C:/Program Files/SoftKinetic/DepthSenseSDK" + "/opt/softkinetic/DepthSenseSDK" + DOC "DepthSense SDK directory") + set(_DSSDK_INCLUDE_DIRS ${DSSDK_DIR}/include) + if(MSVC) + set(DSSDK_LIBRARIES_NAMES DepthSense) + else() + set(DSSDK_LIBRARIES_NAMES DepthSense DepthSensePlugins turbojpeg) + endif() + foreach(LIB ${DSSDK_LIBRARIES_NAMES}) + find_library(DSSDK_LIBRARY_${LIB} + NAMES "${LIB}" + PATHS "${DSSDK_DIR}/lib/" NO_DEFAULT_PATH) + list(APPEND _DSSDK_LIBRARIES ${DSSDK_LIBRARY_${LIB}}) + mark_as_advanced(DSSDK_LIBRARY_${LIB}) + endforeach() + find_package_handle_standard_args(DSSDK DEFAULT_MSG _DSSDK_LIBRARIES _DSSDK_INCLUDE_DIRS) + if(DSSDK_FOUND) + set(DSSDK_LIBRARIES ${_DSSDK_LIBRARIES}) + mark_as_advanced(DSSDK_LIBRARIES) + set(DSSDK_INCLUDE_DIRS ${_DSSDK_INCLUDE_DIRS}) + mark_as_advanced(DSSDK_INCLUDE_DIRS) + endif() +endmacro(find_dssdk) + +macro(find_rssdk) + if(PCL_FIND_QUIETLY) + set(RSSDK_FIND_QUIETLY TRUE) + endif() + if(NOT RSSDK_DIR AND ("@HAVE_RSSDK@" STREQUAL "TRUE")) + get_filename_component(RSSDK_DIR_HINT "@RSSDK_INCLUDE_DIRS@" PATH) + endif() + find_path(RSSDK_DIR include/pxcversion.h + HINTS ${RSSDK_DIR_HINT} + PATHS "$ENV{RSSDK_DIR}" + "$ENV{PROGRAMFILES}/Intel/RSSDK" + "$ENV{PROGRAMW6432}/Intel/RSSDK" + "C:/Program Files (x86)/Intel/RSSDK" + "C:/Program Files/Intel/RSSDK" + DOC "RealSense SDK directory") + set(_RSSDK_INCLUDE_DIRS ${RSSDK_DIR}/include) + set(RSSDK_RELEASE_NAME libpxc.lib) + set(RSSDK_DEBUG_NAME libpxc_d.lib) + find_library(RSSDK_LIBRARY + NAMES ${RSSDK_RELEASE_NAME} + PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH + PATH_SUFFIXES x64 Win32) + find_library(RSSDK_LIBRARY_DEBUG + NAMES ${RSSDK_DEBUG_NAME} ${RSSDK_RELEASE_NAME} + PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH + PATH_SUFFIXES x64 Win32) + if(NOT RSSDK_LIBRARY_DEBUG) + set(RSSDK_LIBRARY_DEBUG ${RSSDK_LIBRARY}) + endif() + set(_RSSDK_LIBRARIES optimized ${RSSDK_LIBRARY} debug ${RSSDK_LIBRARY_DEBUG}) + mark_as_advanced(RSSDK_LIBRARY RSSDK_LIBRARY_DEBUG) + find_package_handle_standard_args(RSSDK DEFAULT_MSG _RSSDK_LIBRARIES _RSSDK_INCLUDE_DIRS) + if(RSSDK_FOUND) + set(RSSDK_LIBRARIES ${_RSSDK_LIBRARIES}) + mark_as_advanced(RSSDK_LIBRARIES) + set(RSSDK_INCLUDE_DIRS ${_RSSDK_INCLUDE_DIRS}) + mark_as_advanced(RSSDK_INCLUDE_DIRS) + endif() +endmacro(find_rssdk) + #remove this as soon as flann is shipped with FindFlann.cmake macro(find_flann) if(PCL_ALL_IN_ONE_INSTALLER) @@ -298,7 +454,11 @@ endmacro(find_flann) macro(find_VTK) if(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID) - set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-5.8") + if(EXISTS "${PCL_ROOT}/3rdParty/VTK/lib/cmake") + set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/cmake/vtk-@VTK_MAJOR_VERSION@.@VTK_MINOR_VERSION@") + else() + set(VTK_DIR "${PCL_ROOT}/3rdParty/VTK/lib/vtk-@VTK_MAJOR_VERSION@.@VTK_MINOR_VERSION@") + endif() elseif(NOT VTK_DIR AND NOT ANDROID) set(VTK_DIR "@VTK_DIR@") endif(PCL_ALL_IN_ONE_INSTALLER AND NOT ANDROID) @@ -356,13 +516,23 @@ IF (WIN32) ELSE (WIN32) IF (APPLE) -# These values for Apple could probably do with improvement. - FIND_PATH( GLEW_INCLUDE_DIR glew.h + + FIND_PATH( GLEW_INCLUDE_DIR GL/glew.h + /usr/local/include /System/Library/Frameworks/GLEW.framework/Versions/A/Headers ${OPENGL_LIBRARY_DIR} ) - SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX") - SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX") + + FIND_LIBRARY( GLEW_GLEW_LIBRARY GLEW + /usr/local/lib + /usr/openwin/lib + /usr/X11R6/lib + ) + + if(NOT GLEW_GLEW_LIBRARY) + SET(GLEW_GLEW_LIBRARY "-framework GLEW" CACHE STRING "GLEW library for OSX") + SET(GLEW_cocoa_LIBRARY "-framework Cocoa" CACHE STRING "Cocoa framework for OSX") + endif() ELSE (APPLE) FIND_PATH( GLEW_INCLUDE_DIR GL/glew.h @@ -442,6 +612,14 @@ macro(find_external_library _component _lib _is_optional) find_openni() elseif("${_lib}" STREQUAL "openni2") find_openni2() + elseif("${_lib}" STREQUAL "ensenso") + find_ensenso() + elseif("${_lib}" STREQUAL "davidSDK") + find_davidSDK() + elseif("${_lib}" STREQUAL "dssdk") + find_dssdk() + elseif("${_lib}" STREQUAL "rssdk") + find_rssdk() elseif("${_lib}" STREQUAL "vtk") find_VTK() elseif("${_lib}" STREQUAL "libusb-1.0") @@ -454,6 +632,7 @@ macro(find_external_library _component _lib _is_optional) string(TOUPPER "${_component}" COMPONENT) string(TOUPPER "${_lib}" LIB) + string(REGEX REPLACE "[.-]" "_" LIB ${LIB}) if(${LIB}_FOUND) list(APPEND PCL_${COMPONENT}_INCLUDE_DIRS ${${LIB}_INCLUDE_DIRS}) if(${LIB}_USE_FILE) @@ -577,7 +756,7 @@ list(LENGTH pcl_all_components PCL_NB_COMPONENTS) @PCLCONFIG_OPTIONAL_DEPENDENCIES@ -set(pcl_header_only_components geometry modeler in_hand_scanner point_cloud_editor cloud_composer optronic_viewer) +set(pcl_header_only_components 2d cuda_common geometry gpu_tracking modeler in_hand_scanner point_cloud_editor cloud_composer optronic_viewer) include(FindPackageHandleStandardArgs) @@ -614,14 +793,21 @@ foreach(component ${PCL_TO_FIND_COMPONENTS}) pcl_message(STATUS "looking for PCL_${COMPONENT}") + string(REGEX REPLACE "^cuda_(.*)$" "\\1" cuda_component "${component}") + string(REGEX REPLACE "^gpu_(.*)$" "\\1" gpu_component "${component}") + find_path(PCL_${COMPONENT}_INCLUDE_DIR NAMES pcl/${component} pcl/apps/${component} + pcl/cuda/${cuda_component} pcl/cuda/${component} + pcl/gpu/${gpu_component} pcl/gpu/${component} HINTS ${PCL_INCLUDE_DIRS} "${PCL_SOURCES_TREE}" PATH_SUFFIXES ${component}/include apps/${component}/include + cuda/${cuda_component}/include + gpu/${gpu_component}/include DOC "path to ${component} headers" NO_DEFAULT_PATH) @@ -711,7 +897,8 @@ endif(NOT "${PCL_DEFINITIONS}" STREQUAL "") pcl_remove_duplicate_libraries(PCL_LIBRARIES PCL_DEDUP_LIBRARIES) set(PCL_LIBRARIES ${PCL_DEDUP_LIBRARIES}) # Add 3rd party libraries, as user code might include our .HPP implementations -list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) +list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${ENSENSO_LIBRARIES} ${davidSDK_LIBRARIES} ${DSSDK_LIBRARIES} ${RSSDK_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) find_package_handle_standard_args(PCL DEFAULT_MSG PCL_LIBRARIES PCL_INCLUDE_DIRS) mark_as_advanced(PCL_LIBRARIES PCL_INCLUDE_DIRS PCL_LIBRARY_DIRS) + diff --git a/README.md b/README.md index 540b304d..8efa2ca1 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,43 @@ -pcl -=== +# Point Cloud Library + + +Continuous integration +---------------------- +[ ![Release] [release-image] ] [releases] +[ ![License] [license-image] ] [license] + +[release-image]: https://img.shields.io/badge/release-1.8.0-green.svg?style=flat +[releases]: https://github.com/PointCloudLibrary/pcl/releases + +[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat +[license]: https://github.com/PointCloudLibrary/pcl/blob/master/LICENSE.txt + +[![Build Status](https://travis-ci.org/PointCloudLibrary/pcl.svg?branch=master)](https://travis-ci.org/PointCloudLibrary/pcl) + +Description +----------- The Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing. PCL is released under the terms of the BSD license, and thus free for commercial and research use. We are financially supported by a consortium of commercial companies, with our own non-profit organization, Open Perception. We would also like to thank individual donors and contributors that have been helping the project. + +Compiling +--------- +Please refer to the platform specific tutorials: + - [Linux](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_posix.php) + - [Mac OS X](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_macosx.php) + - [Microsoft Windows](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_windows.php) + +Documentation +------------- +- [Tutorials](http://www.pointclouds.org/documentation/tutorials/) +- [PCL trunk documentation](http://docs.pointclouds.org/trunk/) (generated 2 times a week) + +Contributing +------------ +Please read [CONTRIBUTING.md](https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md). + +Issues +------ +For general questions on how to use the PCL, please use the [pcl-users](http://www.pcl-users.org/) mailing list (do not forget to subscribe before posting). +To report issues, please read [CONTRIBUTING.md#bug-reports](https://github.com/PointCloudLibrary/pcl/blob/master/CONTRIBUTING.md#bug-reports). diff --git a/apps/3d_rec_framework/CMakeLists.txt b/apps/3d_rec_framework/CMakeLists.txt new file mode 100644 index 00000000..c0e0dc15 --- /dev/null +++ b/apps/3d_rec_framework/CMakeLists.txt @@ -0,0 +1,103 @@ +set(SUBSUBSYS_NAME 3d_rec_framework) +set(SUBSUBSYS_DESC "3D recognition framework") +set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml) + +# Find VTK +if(NOT VTK_FOUND) + set(DEFAULT AUTO_OFF) + set(REASON "VTK was not found.") +else(NOT VTK_FOUND) + set(DEFAULT TRUE) + set(REASON) + include("${VTK_USE_FILE}") +endif(NOT VTK_FOUND) + +# OpenNI found? +if(NOT WITH_OPENNI) + set(DEFAULT AUTO_OFF) + set(REASON "OpenNI was not found or was disabled by the user.") +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif() + +# Default to not building for now +if (${DEFAULT} STREQUAL "TRUE") + set(DEFAULT FALSE) +endif() + +PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS vtk openni) + +if(build) + + include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") + + set(incs_fw "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/normal_estimator.h") + set(incs_fw_global "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/cvfh_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/vfh_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/esf_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/crh_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/global_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global/ourcvfh_estimator.h") + + set(incs_fw_local "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/local_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/fpfh_local_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/fpfh_local_estimator_omp.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/shot_local_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/colorshot_local_estimator.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local/shot_local_estimator_omp.h") + set(incs_pc_source "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/source.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/mesh_source.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source/registered_views_source.h") + + set(incs_pipelines "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_classifier.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_recognizer_crh.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/global_nn_recognizer_cvfh.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/local_recognizer.h") + + set(incc_tools_framework "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools/openni_frame_source.h") + + set(incs_utils "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/metrics.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/persistence_utils.h" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils/vtk_model_sampling.h") + + set(srcs src/pipeline/global_nn_classifier.cpp + src/pipeline/global_nn_recognizer_crh.cpp + src/pipeline/global_nn_recognizer_cvfh.cpp + src/pipeline/local_recognizer.cpp + src/tools/openni_frame_source.cpp) + + set(impl_incs_pipeline "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_classifier.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_recognizer_crh.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/global_nn_recognizer_cvfh.hpp" + "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl/local_recognizer.hpp") + + # Install include files + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper" ${incs_fw}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/global" ${incs_fw_global}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/feature_wrapper/local" ${incs_fw_local}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/tools" ${incc_tools_framework}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline" ${incs_pipelines}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pc_source" ${incs_pc_source}) + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/utils" ${incs_utils}) + + PCL_ADD_INCLUDES("${SUBSUBSYS_NAME}" "${SUBSYS_NAME}/${SUBSUBSYS_NAME}/pipeline/impl" ${impl_incs_pipeline}) + + set(LIB_NAME "pcl_${SUBSUBSYS_NAME}") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSUBSYS_NAME}" ${srcs} ${impl_incs_pipeline} ${incs_utils} ${incs_fw} ${incs_fw_global} ${incs_fw_local} ${incc_tools_framework} ${incs_pipelines} ${incs_pc_source}) + target_link_libraries("${LIB_NAME}" pcl_apps pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search) + + if(WITH_OPENNI) + target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES}) + if(NOT WIN32) + find_package(libusb-1.0 REQUIRED) + target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES}) + endif() + endif() + + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" "" "" "" "" "") + + add_subdirectory(tools) + +endif(build) diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h new file mode 100644 index 00000000..9cb2799d --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h @@ -0,0 +1,94 @@ +/* + * vfh_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_CRH_ESTIMATOR_H_ +#define REC_FRAMEWORK_CRH_ESTIMATOR_H_ + +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class CRHEstimation : public GlobalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using GlobalEstimator::normal_estimator_; + using GlobalEstimator::normals_; + + typename boost::shared_ptr > feature_estimator_; + typedef pcl::PointCloud > CRHPointCloud; + std::vector< CRHPointCloud::Ptr > crh_histograms_; + + public: + + CRHEstimation () + { + + } + + void + setFeatureEstimator(typename boost::shared_ptr > & feature_estimator) { + feature_estimator_ = feature_estimator; + } + + void + estimate (PointInTPtr & in, PointInTPtr & processed, + std::vector, Eigen::aligned_allocator > > & signatures, + std::vector > & centroids) + { + + if (!feature_estimator_) + { + PCL_ERROR("CRHEstimation needs a global estimator... please provide one\n"); + return; + } + + feature_estimator_->estimate(in, processed, signatures, centroids); + + if(!feature_estimator_->computedNormals()) { + normals_.reset(new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals_); + } else { + feature_estimator_->getNormals(normals_); + } + + crh_histograms_.resize(signatures.size()); + + typedef typename pcl::CRHEstimation > CRHEstimation; + CRHEstimation crh; + crh.setInputCloud(processed); + crh.setInputNormals(normals_); + + for (size_t idx = 0; idx < signatures.size (); idx++) + { + Eigen::Vector4f centroid4f(centroids[idx][0],centroids[idx][1],centroids[idx][2],0); + crh.setCentroid(centroid4f); + crh_histograms_[idx].reset(new CRHPointCloud()); + crh.compute (*crh_histograms_[idx]); + } + + } + + void getCRHHistograms(std::vector< CRHPointCloud::Ptr > & crh_histograms) { + crh_histograms = crh_histograms_; + } + + bool + computedNormals () + { + return true; + } + }; + } +} + +#endif /* REC_FRAMEWORK_CVFH_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h new file mode 100644 index 00000000..ec72062d --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/cvfh_estimator.h @@ -0,0 +1,166 @@ +/* + * vfh_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_CVFH_ESTIMATOR_H_ +#define REC_FRAMEWORK_CVFH_ESTIMATOR_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class CVFHEstimation : public GlobalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using GlobalEstimator::normal_estimator_; + using GlobalEstimator::normals_; + float eps_angle_threshold_; + float curvature_threshold_; + float cluster_tolerance_factor_; + bool normalize_bins_; + bool adaptative_MLS_; + + public: + + CVFHEstimation () + { + eps_angle_threshold_ = 0.13f; + curvature_threshold_ = 0.035f; + normalize_bins_ = true; + cluster_tolerance_factor_ = 3.f; + } + + void setCVFHParams(float p1, float p2, float p3) { + eps_angle_threshold_ = p1; + curvature_threshold_ = p2; + cluster_tolerance_factor_ = p3; + } + + void setAdaptativeMLS(bool b) { + adaptative_MLS_ = b; + } + + void + estimate (PointInTPtr & in, PointInTPtr & processed, + typename pcl::PointCloud::CloudVectorType & signatures, + std::vector > & centroids) + { + + if (!normal_estimator_) + { + PCL_ERROR("This feature needs normals... please provide a normal estimator\n"); + return; + } + + pcl::MovingLeastSquares mls; + if(adaptative_MLS_) { + typename search::KdTree::Ptr tree; + Eigen::Vector4f centroid_cluster; + pcl::compute3DCentroid (*in, centroid_cluster); + float dist_to_sensor = centroid_cluster.norm(); + float sigma = dist_to_sensor * 0.01f; + mls.setSearchMethod(tree); + mls.setSearchRadius (sigma); + mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE); + mls.setUpsamplingRadius (0.002); + mls.setUpsamplingStepSize (0.001); + } + + normals_.reset (new pcl::PointCloud); + { + normal_estimator_->estimate (in, processed, normals_); + } + + if(adaptative_MLS_) { + mls.setInputCloud(processed); + + PointInTPtr filtered(new pcl::PointCloud); + mls.process(*filtered); + + processed.reset(new pcl::PointCloud); + normals_.reset (new pcl::PointCloud); + { + filtered->is_dense = false; + normal_estimator_->estimate (filtered, processed, normals_); + } + } + + /*normals_.reset(new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals_);*/ + + typedef typename pcl::CVFHEstimation CVFHEstimation; + pcl::PointCloud cvfh_signatures; + typename pcl::search::KdTree::Ptr cvfh_tree (new pcl::search::KdTree); + + CVFHEstimation cvfh; + cvfh.setSearchMethod (cvfh_tree); + cvfh.setInputCloud (processed); + cvfh.setInputNormals (normals_); + + cvfh.setEPSAngleThreshold (eps_angle_threshold_); + cvfh.setCurvatureThreshold (curvature_threshold_); + cvfh.setNormalizeBins (normalize_bins_); + + float radius = normal_estimator_->normal_radius_; + float cluster_tolerance_radius = normal_estimator_->grid_resolution_ * cluster_tolerance_factor_; + + if (normal_estimator_->compute_mesh_resolution_) + { + radius = normal_estimator_->mesh_resolution_ * normal_estimator_->factor_normals_; + cluster_tolerance_radius = normal_estimator_->mesh_resolution_ * cluster_tolerance_factor_; + + if (normal_estimator_->do_voxel_grid_) + { + radius *= normal_estimator_->factor_voxel_grid_; + cluster_tolerance_radius *= normal_estimator_->factor_voxel_grid_; + } + } + + cvfh.setClusterTolerance (cluster_tolerance_radius); + cvfh.setRadiusNormals (radius); + cvfh.setMinPoints (100); + + //std::cout << "Res:" << normal_estimator_->mesh_resolution_ << " Radius normals:" << radius << " Cluster tolerance:" << cluster_tolerance_radius << " " << eps_angle_threshold_ << " " << curvature_threshold_ << std::endl; + + cvfh.compute (cvfh_signatures); + + for (size_t i = 0; i < cvfh_signatures.points.size (); i++) + { + pcl::PointCloud vfh_signature; + vfh_signature.points.resize (1); + vfh_signature.width = vfh_signature.height = 1; + for (int d = 0; d < 308; ++d) + vfh_signature.points[0].histogram[d] = cvfh_signatures.points[i].histogram[d]; + + signatures.push_back (vfh_signature); + } + + cvfh.getCentroidClusters (centroids); + //std::cout << "centroids size:" << centroids.size () << std::endl; + + } + + bool + computedNormals () + { + return true; + } + + void setNormalizeBins(bool b) { + normalize_bins_ = b; + } + }; + } +} + +#endif /* REC_FRAMEWORK_CVFH_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/esf_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/esf_estimator.h new file mode 100644 index 00000000..2811f31c --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/esf_estimator.h @@ -0,0 +1,60 @@ +/* + * vfh_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_ESF_ESTIMATOR_H_ +#define REC_FRAMEWORK_ESF_ESTIMATOR_H_ + +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class ESFEstimation : public GlobalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + + public: + void + estimate (PointInTPtr & in, PointInTPtr & processed, + typename pcl::PointCloud::CloudVectorType & signatures, + std::vector > & centroids) + { + + typedef typename pcl::ESFEstimation ESFEstimation; + pcl::PointCloud ESF_signature; + + ESFEstimation esf; + esf.setInputCloud (in); + esf.compute (ESF_signature); + + signatures.resize (1); + centroids.resize (1); + + signatures[0] = ESF_signature; + + Eigen::Vector4f centroid4f; + pcl::compute3DCentroid (*in, centroid4f); + centroids[0] = Eigen::Vector3f (centroid4f[0], centroid4f[1], centroid4f[2]); + + pcl::copyPointCloud(*in, *processed); + //processed = in; + } + + bool + computedNormals () + { + return false; + } + }; + } +} + +#endif /* REC_FRAMEWORK_ESF_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h new file mode 100644 index 00000000..6bc63044 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/global_estimator.h @@ -0,0 +1,48 @@ +/* + * estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_GLOBAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_GLOBAL_ESTIMATOR_H_ + +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class GlobalEstimator { + protected: + bool computed_normals_; + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + + typename boost::shared_ptr > normal_estimator_; + + pcl::PointCloud::Ptr normals_; + + public: + virtual void + estimate (PointInTPtr & in, PointInTPtr & processed, std::vector, Eigen::aligned_allocator< + pcl::PointCloud > > & signatures, std::vector > & centroids)=0; + + virtual bool computedNormals() = 0; + + void setNormalEstimator(boost::shared_ptr > & ne) { + normal_estimator_ = ne; + } + + void getNormals(pcl::PointCloud::Ptr & normals) { + normals = normals_; + } + + }; + } +} + + +#endif /* REC_FRAMEWORK_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h new file mode 100644 index 00000000..c9b7ef2d --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/ourcvfh_estimator.h @@ -0,0 +1,201 @@ +/* + * vfh_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_ +#define REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class OURCVFHEstimator : public GlobalEstimator + { + + protected: + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using GlobalEstimator::normal_estimator_; + using GlobalEstimator::normals_; + float eps_angle_threshold_; + float curvature_threshold_; + float cluster_tolerance_factor_; + bool normalize_bins_; + bool adaptative_MLS_; + float refine_factor_; + + std::vector valid_roll_transforms_; + std::vector > transforms_; + std::vector cluster_indices_; + + public: + + OURCVFHEstimator () + { + eps_angle_threshold_ = 0.13f; + curvature_threshold_ = 0.035f; + normalize_bins_ = true; + cluster_tolerance_factor_ = 3.f; + adaptative_MLS_ = false; + } + + void + setCVFHParams (float p1, float p2, float p3) + { + eps_angle_threshold_ = p1; + curvature_threshold_ = p2; + cluster_tolerance_factor_ = p3; + } + + void + setAdaptativeMLS (bool b) + { + adaptative_MLS_ = b; + } + + void + setRefineClustersParam (float p4) + { + refine_factor_ = p4; + } + + void + getValidTransformsVec (std::vector & valid) + { + valid = valid_roll_transforms_; + } + + void + getTransformsVec (std::vector > & trans) + { + trans = transforms_; + } + + virtual void + estimate (PointInTPtr & in, PointInTPtr & processed, typename pcl::PointCloud::CloudVectorType & signatures, + std::vector > & centroids) + { + + valid_roll_transforms_.clear (); + transforms_.clear (); + + if (!normal_estimator_) + { + PCL_ERROR("This feature needs normals... please provide a normal estimator\n"); + return; + } + + pcl::MovingLeastSquares mls; + if (adaptative_MLS_) + { + typename search::KdTree::Ptr tree; + Eigen::Vector4f centroid_cluster; + pcl::compute3DCentroid (*in, centroid_cluster); + float dist_to_sensor = centroid_cluster.norm (); + float sigma = dist_to_sensor * 0.01f; + mls.setSearchMethod (tree); + mls.setSearchRadius (sigma); + mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE); + mls.setUpsamplingRadius (0.002); + mls.setUpsamplingStepSize (0.001); + } + + normals_.reset (new pcl::PointCloud); + { + normal_estimator_->estimate (in, processed, normals_); + } + + if (adaptative_MLS_) + { + mls.setInputCloud (processed); + + PointInTPtr filtered (new pcl::PointCloud); + mls.process (*filtered); + + processed.reset (new pcl::PointCloud); + normals_.reset (new pcl::PointCloud); + { + filtered->is_dense = false; + normal_estimator_->estimate (filtered, processed, normals_); + } + } + + /*normals_.reset(new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals_);*/ + + typedef typename pcl::OURCVFHEstimation OURCVFHEstimation; + pcl::PointCloud cvfh_signatures; + typename pcl::search::KdTree::Ptr cvfh_tree (new pcl::search::KdTree); + + OURCVFHEstimation cvfh; + cvfh.setSearchMethod (cvfh_tree); + cvfh.setInputCloud (processed); + cvfh.setInputNormals (normals_); + cvfh.setEPSAngleThreshold (eps_angle_threshold_); + cvfh.setCurvatureThreshold (curvature_threshold_); + cvfh.setNormalizeBins (normalize_bins_); + cvfh.setRefineClusters(refine_factor_); + + float radius = normal_estimator_->normal_radius_; + float cluster_tolerance_radius = normal_estimator_->grid_resolution_ * cluster_tolerance_factor_; + + if (normal_estimator_->compute_mesh_resolution_) + { + radius = normal_estimator_->mesh_resolution_ * normal_estimator_->factor_normals_; + cluster_tolerance_radius = normal_estimator_->mesh_resolution_ * cluster_tolerance_factor_; + + if (normal_estimator_->do_voxel_grid_) + { + radius *= normal_estimator_->factor_voxel_grid_; + cluster_tolerance_radius *= normal_estimator_->factor_voxel_grid_; + } + } + + cvfh.setClusterTolerance (cluster_tolerance_radius); + cvfh.setRadiusNormals (radius); + cvfh.setMinPoints (100); + cvfh.compute (cvfh_signatures); + + //std::cout << "Res:" << normal_estimator_->mesh_resolution_ << " Radius normals:" << radius << " Cluster tolerance:" << cluster_tolerance_radius << " " << eps_angle_threshold_ << " " << curvature_threshold_ << std::endl; + + for (size_t i = 0; i < cvfh_signatures.points.size (); i++) + { + pcl::PointCloud vfh_signature; + vfh_signature.points.resize (1); + vfh_signature.width = vfh_signature.height = 1; + for (int d = 0; d < 308; ++d) + vfh_signature.points[0].histogram[d] = cvfh_signatures.points[i].histogram[d]; + + signatures.push_back (vfh_signature); + } + + cvfh.getCentroidClusters (centroids); + cvfh.getTransforms(transforms_); + cvfh.getValidTransformsVec(valid_roll_transforms_); + cvfh.getClusterIndices(cluster_indices_); + } + + bool + computedNormals () + { + return true; + } + + void + setNormalizeBins (bool b) + { + normalize_bins_ = b; + } + }; + } +} + +#endif /* REC_FRAMEWORK_OURCVFH_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/vfh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/vfh_estimator.h new file mode 100644 index 00000000..e3efc962 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/vfh_estimator.h @@ -0,0 +1,75 @@ +/* + * vfh_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_VFH_ESTIMATOR_H_ +#define REC_FRAMEWORK_VFH_ESTIMATOR_H_ + +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class VFHEstimation : public GlobalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using GlobalEstimator::normal_estimator_; + using GlobalEstimator::normals_; + + public: + void + estimate (PointInTPtr & in, PointInTPtr & processed, + typename pcl::PointCloud::CloudVectorType & signatures, + std::vector > & centroids) + { + + if (!normal_estimator_) + { + PCL_ERROR("This feature needs normals... please provide a normal estimator\n"); + return; + } + + normals_.reset(new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals_); + + typedef typename pcl::VFHEstimation VFHEstimation; + pcl::PointCloud vfh_signature; + + VFHEstimation vfh; + typename pcl::search::KdTree::Ptr vfh_tree (new pcl::search::KdTree); + vfh.setSearchMethod (vfh_tree); + vfh.setInputCloud (processed); + vfh.setInputNormals (normals_); + vfh.setNormalizeBins (true); + vfh.setNormalizeDistance (true); + vfh.compute (vfh_signature); + + signatures.resize (1); + centroids.resize (1); + + signatures[0] = vfh_signature; + + Eigen::Vector4f centroid4f; + pcl::compute3DCentroid (*in, centroid4f); + centroids[0] = Eigen::Vector3f (centroid4f[0], centroid4f[1], centroid4f[2]); + + } + + bool + computedNormals () + { + return true; + } + }; + } +} + +#endif /* REC_FRAMEWORK_VFH_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/colorshot_local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/colorshot_local_estimator.h new file mode 100644 index 00000000..cb7f619f --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/colorshot_local_estimator.h @@ -0,0 +1,94 @@ +/* + * shot_local_estimator.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class ColorSHOTLocalEstimation : public LocalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + + using LocalEstimator::support_radius_; + using LocalEstimator::normal_estimator_; + using LocalEstimator::keypoint_extractor_; + + public: + bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) + { + + if (!normal_estimator_) + { + PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n"); + return false; + } + + if (keypoint_extractor_.size() == 0) + { + PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n"); + return false; + } + + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + normals.reset (new pcl::PointCloud); + { + pcl::ScopeTime t ("Compute normals"); + normal_estimator_->estimate (in, processed, normals); + } + + //compute keypoints + computeKeypoints(processed, keypoints, normals); + + if (keypoints->points.size () == 0) + { + PCL_WARN("ColorSHOTLocalEstimation :: No keypoints were found\n"); + return false; + } + + //compute signatures + typedef typename pcl::SHOTColorEstimation SHOTEstimator; + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + + pcl::PointCloud::Ptr shots (new pcl::PointCloud); + SHOTEstimator shot_estimate; + shot_estimate.setSearchMethod (tree); + shot_estimate.setInputNormals (normals); + shot_estimate.setInputCloud (keypoints); + shot_estimate.setSearchSurface(processed); + shot_estimate.setRadiusSearch (support_radius_); + shot_estimate.compute (*shots); + signatures->resize (shots->points.size ()); + signatures->width = static_cast (shots->points.size ()); + signatures->height = 1; + + int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float); + + for (size_t k = 0; k < shots->points.size (); k++) + for (int i = 0; i < size_feat; i++) + signatures->points[k].histogram[i] = shots->points[k].descriptor[i]; + + return true; + + } + + }; + } +} + +#endif /* REC_FRAMEWORK_COLORSHOT_LOCAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h new file mode 100644 index 00000000..61423c0b --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator.h @@ -0,0 +1,93 @@ +/* + * shot_local_estimator.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ + +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class FPFHLocalEstimation : public LocalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + typedef pcl::PointCloud KeypointCloud; + + using LocalEstimator::support_radius_; + using LocalEstimator::normal_estimator_; + using LocalEstimator::keypoint_extractor_; + + public: + bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) + { + + if (!normal_estimator_) + { + PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n"); + return false; + } + + if (keypoint_extractor_.size() == 0) + { + PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n"); + return false; + } + + //compute normals + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals); + + this->computeKeypoints(processed, keypoints, normals); + std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl; + + if (keypoints->points.size () == 0) + { + PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n"); + return false; + } + + assert (processed->points.size () == normals->points.size ()); + + //compute signatures + typedef typename pcl::FPFHEstimation FPFHEstimator; + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + + pcl::PointCloud::Ptr fpfhs (new pcl::PointCloud); + FPFHEstimator fpfh_estimate; + fpfh_estimate.setSearchMethod (tree); + fpfh_estimate.setInputCloud (keypoints); + fpfh_estimate.setSearchSurface(processed); + fpfh_estimate.setInputNormals (normals); + fpfh_estimate.setRadiusSearch (support_radius_); + fpfh_estimate.compute (*fpfhs); + + signatures->resize (fpfhs->points.size ()); + signatures->width = static_cast (fpfhs->points.size ()); + signatures->height = 1; + + int size_feat = 33; + for (size_t k = 0; k < fpfhs->points.size (); k++) + for (int i = 0; i < size_feat; i++) + signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i]; + + return true; + + } + + }; + } +} + +#endif /* REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator_omp.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator_omp.h new file mode 100644 index 00000000..1bdf3ea0 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/fpfh_local_estimator_omp.h @@ -0,0 +1,95 @@ +/* + * shot_local_estimator.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_OMP_H_ +#define REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_OMP_H_ + +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class FPFHLocalEstimationOMP : public LocalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + typedef pcl::PointCloud KeypointCloud; + + using LocalEstimator::support_radius_; + using LocalEstimator::normal_estimator_; + using LocalEstimator::keypoint_extractor_; + + public: + bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) + { + + if (!normal_estimator_) + { + PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n"); + return false; + } + + if (keypoint_extractor_.size() == 0) + { + PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n"); + return false; + } + + //compute normals + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + normal_estimator_->estimate (in, processed, normals); + + //compute keypoints + computeKeypoints(processed, keypoints, normals); + std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl; + + if (keypoints->points.size () == 0) + { + PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n"); + return false; + } + + assert (processed->points.size () == normals->points.size ()); + + //compute signatures + typedef typename pcl::FPFHEstimationOMP FPFHEstimator; + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + + pcl::PointCloud::Ptr fpfhs (new pcl::PointCloud); + FPFHEstimator fpfh_estimate; + fpfh_estimate.setNumberOfThreads(8); + fpfh_estimate.setSearchMethod (tree); + fpfh_estimate.setInputCloud (keypoints); + fpfh_estimate.setSearchSurface(processed); + fpfh_estimate.setInputNormals (normals); + fpfh_estimate.setRadiusSearch (support_radius_); + fpfh_estimate.compute (*fpfhs); + + signatures->resize (fpfhs->points.size ()); + signatures->width = static_cast (fpfhs->points.size ()); + signatures->height = 1; + + int size_feat = 33; + for (size_t k = 0; k < fpfhs->points.size (); k++) + for (int i = 0; i < size_feat; i++) + signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i]; + + return true; + + } + + }; + } +} + +#endif /* REC_FRAMEWORK_FPFH_LOCAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h new file mode 100644 index 00000000..a5b23b95 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h @@ -0,0 +1,526 @@ +/* + * estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_LOCAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_LOCAL_ESTIMATOR_H_ + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + template<> + struct SIFTKeypointFieldSelector + { + inline float + operator () (const PointXYZ & p) const + { + return p.z; + } + }; +} + +namespace pcl +{ + namespace rec_3d_framework + { + + template + class KeypointExtractor + { + protected: + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr PointOutTPtr; + typename pcl::PointCloud::Ptr input_; + float radius_; + + public: + void + setInputCloud (PointInTPtr & input) + { + input_ = input; + } + + void + setSupportRadius (float f) + { + radius_ = f; + } + + virtual void + compute (PointOutTPtr & keypoints) = 0; + + virtual void + setNormals (const pcl::PointCloud::Ptr & /*normals*/) + { + + } + + virtual bool + needNormals () + { + return false; + } + + }; + + template + class UniformSamplingExtractor : public KeypointExtractor + { + private: + typedef typename pcl::PointCloud::Ptr PointInTPtr; + bool filter_planar_; + using KeypointExtractor::input_; + using KeypointExtractor::radius_; + float sampling_density_; + boost::shared_ptr > > neighborhood_indices_; + boost::shared_ptr > > neighborhood_dist_; + + void + filterPlanar (PointInTPtr & input, PointInTPtr & keypoints_cloud) + { + pcl::PointCloud filtered_keypoints; + //create a search object + typename pcl::search::Search::Ptr tree; + if (input->isOrganized ()) + tree.reset (new pcl::search::OrganizedNeighbor ()); + else + tree.reset (new pcl::search::KdTree (false)); + tree->setInputCloud (input); + + neighborhood_indices_.reset (new std::vector >); + neighborhood_indices_->resize (keypoints_cloud->points.size ()); + neighborhood_dist_.reset (new std::vector >); + neighborhood_dist_->resize (keypoints_cloud->points.size ()); + + filtered_keypoints.points.resize (keypoints_cloud->points.size ()); + int good = 0; + + for (size_t i = 0; i < keypoints_cloud->points.size (); i++) + { + + if (tree->radiusSearch (keypoints_cloud->points[i], radius_, (*neighborhood_indices_)[good], (*neighborhood_dist_)[good])) + { + + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + EIGEN_ALIGN16 Eigen::Vector3f eigenValues; + EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors; + + //compute planarity of the region + computeMeanAndCovarianceMatrix (*input, (*neighborhood_indices_)[good], covariance_matrix, xyz_centroid); + pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues); + + float eigsum = eigenValues.sum (); + if (!pcl_isfinite(eigsum)) + { + PCL_ERROR("Eigen sum is not finite\n"); + } + + if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2)) + { + //region is not planar, add to filtered keypoint + keypoints_cloud->points[good] = keypoints_cloud->points[i]; + good++; + } + } + } + + neighborhood_indices_->resize (good); + neighborhood_dist_->resize (good); + keypoints_cloud->points.resize (good); + + neighborhood_indices_->clear (); + neighborhood_dist_->clear (); + + } + + public: + + void + setFilterPlanar (bool b) + { + filter_planar_ = b; + } + + void + setSamplingDensity (float f) + { + sampling_density_ = f; + } + + void + compute (PointInTPtr & keypoints) + { + keypoints.reset (new pcl::PointCloud); + + pcl::UniformSampling keypoint_extractor; + keypoint_extractor.setRadiusSearch (sampling_density_); + keypoint_extractor.setInputCloud (input_); + + keypoint_extractor.filter (*keypoints); + + if (filter_planar_) + filterPlanar (input_, keypoints); + } + }; + + template + class SIFTKeypointExtractor : public KeypointExtractor + { + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using KeypointExtractor::input_; + using KeypointExtractor::radius_; + + public: + void + compute (PointInTPtr & keypoints) + { + keypoints.reset (new pcl::PointCloud); + + typename pcl::PointCloud::Ptr intensity_keypoints (new pcl::PointCloud); + pcl::SIFTKeypoint sift3D; + sift3D.setScales (0.003f, 3, 2); + sift3D.setMinimumContrast (0.1f); + sift3D.setInputCloud (input_); + sift3D.setSearchSurface (input_); + sift3D.compute (*intensity_keypoints); + pcl::copyPointCloud (*intensity_keypoints, *keypoints); + } + }; + + template + class SIFTSurfaceKeypointExtractor : public KeypointExtractor + { + typedef typename pcl::PointCloud::Ptr PointInTPtr; + pcl::PointCloud::Ptr normals_; + using KeypointExtractor::input_; + using KeypointExtractor::radius_; + + bool + needNormals () + { + return true; + } + + void + setNormals (const pcl::PointCloud::Ptr & normals) + { + normals_ = normals; + } + + public: + void + compute (PointInTPtr & keypoints) + { + if (normals_ == 0 || (normals_->points.size () != input_->points.size ())) + PCL_WARN("SIFTSurfaceKeypointExtractor -- Normals are not valid\n"); + + keypoints.reset (new pcl::PointCloud); + + typename pcl::PointCloud::Ptr input_cloud (new pcl::PointCloud); + input_cloud->width = input_->width; + input_cloud->height = input_->height; + input_cloud->points.resize (input_->width * input_->height); + for (size_t i = 0; i < input_->points.size (); i++) + { + input_cloud->points[i].getVector3fMap () = input_->points[i].getVector3fMap (); + input_cloud->points[i].getNormalVector3fMap () = normals_->points[i].getNormalVector3fMap (); + } + + typename pcl::PointCloud::Ptr intensity_keypoints (new pcl::PointCloud); + pcl::SIFTKeypoint sift3D; + sift3D.setScales (0.003f, 3, 2); + sift3D.setMinimumContrast (0.0); + sift3D.setInputCloud (input_cloud); + sift3D.setSearchSurface (input_cloud); + sift3D.compute (*intensity_keypoints); + pcl::copyPointCloud (*intensity_keypoints, *keypoints); + } + }; + + template + class HarrisKeypointExtractor : public KeypointExtractor + { + + pcl::PointCloud::Ptr normals_; + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using KeypointExtractor::input_; + using KeypointExtractor::radius_; + typename pcl::HarrisKeypoint3D::ResponseMethod m_; + float non_max_radius_; + float threshold_; + + public: + + HarrisKeypointExtractor () + { + m_ = pcl::HarrisKeypoint3D::HARRIS; + non_max_radius_ = 0.01f; + threshold_ = 0.f; + } + + bool + needNormals () + { + return true; + } + + void + setNormals (const pcl::PointCloud::Ptr & normals) + { + normals_ = normals; + } + + void + setThreshold(float t) { + threshold_ = t; + } + + void + setResponseMethod (typename pcl::HarrisKeypoint3D::ResponseMethod m) + { + m_ = m; + } + + void + setNonMaximaRadius(float r) { + non_max_radius_ = r; + } + + void + compute (PointInTPtr & keypoints) + { + keypoints.reset (new pcl::PointCloud); + + if (normals_ == 0 || (normals_->points.size () != input_->points.size ())) + PCL_WARN("HarrisKeypointExtractor -- Normals are not valid\n"); + + typename pcl::PointCloud::Ptr intensity_keypoints (new pcl::PointCloud); + + pcl::HarrisKeypoint3D harris; + harris.setNonMaxSupression (true); + harris.setRefine (false); + harris.setThreshold (threshold_); + harris.setInputCloud (input_); + harris.setNormals (normals_); + harris.setRadius (non_max_radius_); + harris.setRadiusSearch (non_max_radius_); + harris.setMethod (m_); + harris.compute (*intensity_keypoints); + + pcl::copyPointCloud (*intensity_keypoints, *keypoints); + } + }; + + template + class SUSANKeypointExtractor : public KeypointExtractor + { + + pcl::PointCloud::Ptr normals_; + typedef typename pcl::PointCloud::Ptr PointInTPtr; + using KeypointExtractor::input_; + using KeypointExtractor::radius_; + + public: + + SUSANKeypointExtractor () + { + + } + + bool + needNormals () + { + return true; + } + + void + setNormals (const pcl::PointCloud::Ptr & normals) + { + normals_ = normals; + } + + void + compute (PointInTPtr & keypoints) + { + keypoints.reset (new pcl::PointCloud); + + if (normals_ == 0 || (normals_->points.size () != input_->points.size ())) + PCL_WARN("SUSANKeypointExtractor -- Normals are not valid\n"); + + typename pcl::PointCloud::Ptr intensity_keypoints (new pcl::PointCloud); + + pcl::SUSANKeypoint susan; + susan.setNonMaxSupression (true); + susan.setInputCloud (input_); + susan.setNormals (normals_); + susan.setRadius (0.01f); + susan.setRadiusSearch (0.01f); + susan.compute (*intensity_keypoints); + + pcl::copyPointCloud (*intensity_keypoints, *keypoints); + } + }; + + template + class LocalEstimator + { + protected: + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + + typename boost::shared_ptr > normal_estimator_; + //typename boost::shared_ptr > keypoint_extractor_; + std::vector > > keypoint_extractor_; //this should be a vector + float support_radius_; + //bool filter_planar_; + + bool adaptative_MLS_; + + boost::shared_ptr > > neighborhood_indices_; + boost::shared_ptr > > neighborhood_dist_; + + //std::vector< std::vector > neighborhood_indices_; + //std::vector< std::vector > neighborhood_dist_; + + void + computeKeypoints (PointInTPtr & cloud, PointInTPtr & keypoints, pcl::PointCloud::Ptr & normals) + { + keypoints.reset (new pcl::PointCloud); + for (size_t i = 0; i < keypoint_extractor_.size (); i++) + { + keypoint_extractor_[i]->setInputCloud (cloud); + if (keypoint_extractor_[i]->needNormals ()) + keypoint_extractor_[i]->setNormals (normals); + + keypoint_extractor_[i]->setSupportRadius (support_radius_); + + PointInTPtr detected_keypoints; + keypoint_extractor_[i]->compute (detected_keypoints); + *keypoints += *detected_keypoints; + } + } + + public: + + LocalEstimator () + { + adaptative_MLS_ = false; + keypoint_extractor_.clear (); + } + + void + setAdaptativeMLS (bool b) + { + adaptative_MLS_ = b; + } + + virtual bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)=0; + + void + setNormalEstimator (boost::shared_ptr > & ne) + { + normal_estimator_ = ne; + } + + /** + * \brief Right now only uniformSampling keypoint extractor is allowed + */ + void + addKeypointExtractor (boost::shared_ptr > & ke) + { + keypoint_extractor_.push_back (ke); + } + + void + setKeypointExtractors (std::vector > > & ke) + { + keypoint_extractor_ = ke; + } + + void + setSupportRadius (float r) + { + support_radius_ = r; + } + + /*void + setFilterPlanar (bool b) + { + filter_planar_ = b; + } + + void + filterPlanar (PointInTPtr & input, KeypointCloud & keypoints_cloud) + { + pcl::PointCloud filtered_keypoints; + //create a search object + typename pcl::search::Search::Ptr tree; + if (input->isOrganized ()) + tree.reset (new pcl::search::OrganizedNeighbor ()); + else + tree.reset (new pcl::search::KdTree (false)); + tree->setInputCloud (input); + + //std::vector nn_indices; + //std::vector nn_distances; + + neighborhood_indices_.reset (new std::vector >); + neighborhood_indices_->resize (keypoints_cloud.points.size ()); + neighborhood_dist_.reset (new std::vector >); + neighborhood_dist_->resize (keypoints_cloud.points.size ()); + + filtered_keypoints.points.resize (keypoints_cloud.points.size ()); + int good = 0; + + //#pragma omp parallel for num_threads(8) + for (size_t i = 0; i < keypoints_cloud.points.size (); i++) + { + + if (tree->radiusSearch (keypoints_cloud[i], support_radius_, (*neighborhood_indices_)[good], (*neighborhood_dist_)[good])) + { + + EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f xyz_centroid; + EIGEN_ALIGN16 Eigen::Vector3f eigenValues; + EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors; + + //compute planarity of the region + computeMeanAndCovarianceMatrix (*input, (*neighborhood_indices_)[good], covariance_matrix, xyz_centroid); + pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues); + + float eigsum = eigenValues.sum (); + if (!pcl_isfinite(eigsum)) + { + PCL_ERROR("Eigen sum is not finite\n"); + } + + if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2)) + { + //region is not planar, add to filtered keypoint + keypoints_cloud.points[good] = keypoints_cloud.points[i]; + good++; + } + } + } + + neighborhood_indices_->resize (good); + neighborhood_dist_->resize (good); + keypoints_cloud.points.resize (good); + }*/ + + }; + } +} + +#endif /* REC_FRAMEWORK_LOCAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h new file mode 100644 index 00000000..74e5f635 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator.h @@ -0,0 +1,144 @@ +/* + * shot_local_estimator.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class SHOTLocalEstimation : public LocalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + + using LocalEstimator::support_radius_; + using LocalEstimator::normal_estimator_; + using LocalEstimator::keypoint_extractor_; + using LocalEstimator::adaptative_MLS_; + + public: + bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) + { + + if (!normal_estimator_) + { + PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n"); + return false; + } + + if (keypoint_extractor_.size() == 0) + { + PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n"); + return false; + } + + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + pcl::MovingLeastSquares mls; + if(adaptative_MLS_) { + typename search::KdTree::Ptr tree; + Eigen::Vector4f centroid_cluster; + pcl::compute3DCentroid (*in, centroid_cluster); + float dist_to_sensor = centroid_cluster.norm(); + float sigma = dist_to_sensor * 0.01f; + mls.setSearchMethod(tree); + mls.setSearchRadius (sigma); + mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE); + mls.setUpsamplingRadius (0.002); + mls.setUpsamplingStepSize (0.001); + } + + normals.reset (new pcl::PointCloud); + { + pcl::ScopeTime t ("Compute normals"); + normal_estimator_->estimate (in, processed, normals); + } + + if(adaptative_MLS_) { + mls.setInputCloud(processed); + + PointInTPtr filtered(new pcl::PointCloud); + mls.process(*filtered); + + processed.reset(new pcl::PointCloud); + normals.reset (new pcl::PointCloud); + { + pcl::ScopeTime t ("Compute normals after MLS"); + filtered->is_dense = false; + normal_estimator_->estimate (filtered, processed, normals); + } + } + + //compute normals + //pcl::PointCloud::Ptr normals (new pcl::PointCloud); + //normal_estimator_->estimate (in, processed, normals); + + //compute keypoints + this->computeKeypoints(processed, keypoints, normals); + std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl; + + //compute keypoints + /*keypoint_extractor_->setInputCloud (processed); + if(keypoint_extractor_->needNormals()) + keypoint_extractor_->setNormals(normals); + + std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl; + + keypoint_extractor_->setSupportRadius(support_radius_); + keypoint_extractor_->compute (keypoints);*/ + + if (keypoints->points.size () == 0) + { + PCL_WARN("SHOTLocalEstimation :: No keypoints were found\n"); + return false; + } + + std::cout << keypoints->points.size() << " " << normals->points.size() << " " << processed->points.size() << std::endl; + //compute signatures + typedef typename pcl::SHOTEstimation SHOTEstimator; + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + + pcl::PointCloud::Ptr shots (new pcl::PointCloud); + + SHOTEstimator shot_estimate; + shot_estimate.setSearchMethod (tree); + shot_estimate.setInputCloud (keypoints); + shot_estimate.setSearchSurface(processed); + shot_estimate.setInputNormals (normals); + shot_estimate.setRadiusSearch (support_radius_); + shot_estimate.compute (*shots); + signatures->resize (shots->points.size ()); + signatures->width = static_cast (shots->points.size ()); + signatures->height = 1; + + int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float); + + for (size_t k = 0; k < shots->points.size (); k++) + for (int i = 0; i < size_feat; i++) + signatures->points[k].histogram[i] = shots->points[k].descriptor[i]; + + return true; + + } + + private: + + + }; + } +} + +#endif /* REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h new file mode 100644 index 00000000..4c52786f --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/shot_local_estimator_omp.h @@ -0,0 +1,156 @@ +/* + * shot_local_estimator.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_ +#define REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class SHOTLocalEstimationOMP : public LocalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::Ptr FeatureTPtr; + typedef pcl::PointCloud KeypointCloud; + + using LocalEstimator::support_radius_; + using LocalEstimator::normal_estimator_; + using LocalEstimator::keypoint_extractor_; + using LocalEstimator::neighborhood_indices_; + using LocalEstimator::neighborhood_dist_; + using LocalEstimator::adaptative_MLS_; + + public: + bool + estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures) + { + if (!normal_estimator_) + { + PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs normals... please provide a normal estimator\n"); + return false; + } + + if (keypoint_extractor_.size() == 0) + { + PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs a keypoint extractor... please provide one\n"); + return false; + } + + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + pcl::MovingLeastSquares mls; + if (adaptative_MLS_) + { + typename search::KdTree::Ptr tree; + Eigen::Vector4f centroid_cluster; + pcl::compute3DCentroid (*in, centroid_cluster); + float dist_to_sensor = centroid_cluster.norm (); + float sigma = dist_to_sensor * 0.01f; + mls.setSearchMethod (tree); + mls.setSearchRadius (sigma); + mls.setUpsamplingMethod (mls.SAMPLE_LOCAL_PLANE); + mls.setUpsamplingRadius (0.002); + mls.setUpsamplingStepSize (0.001); + } + + normals.reset (new pcl::PointCloud); + { + pcl::ScopeTime t ("Compute normals"); + normal_estimator_->estimate (in, processed, normals); + } + + if (adaptative_MLS_) + { + mls.setInputCloud (processed); + + PointInTPtr filtered (new pcl::PointCloud); + mls.process (*filtered); + + processed.reset (new pcl::PointCloud); + normals.reset (new pcl::PointCloud); + { + pcl::ScopeTime t ("Compute normals after MLS"); + filtered->is_dense = false; + normal_estimator_->estimate (filtered, processed, normals); + } + } + + this->computeKeypoints(processed, keypoints, normals); + std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl; + + if (keypoints->points.size () == 0) + { + PCL_WARN("SHOTLocalEstimationOMP :: No keypoints were found\n"); + return false; + } + + //compute signatures + typedef typename pcl::SHOTEstimationOMP SHOTEstimator; + typename pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (processed); + + pcl::PointCloud::Ptr shots (new pcl::PointCloud); + SHOTEstimator shot_estimate; + shot_estimate.setNumberOfThreads (8); + shot_estimate.setSearchMethod (tree); + shot_estimate.setInputCloud (keypoints); + shot_estimate.setSearchSurface(processed); + shot_estimate.setInputNormals (normals); + shot_estimate.setRadiusSearch (support_radius_); + + { + pcl::ScopeTime t ("Compute SHOT"); + shot_estimate.compute (*shots); + } + + signatures->resize (shots->points.size ()); + signatures->width = static_cast (shots->points.size ()); + signatures->height = 1; + + int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float); + + int good = 0; + for (size_t k = 0; k < shots->points.size (); k++) + { + + int NaNs = 0; + for (int i = 0; i < size_feat; i++) + { + if (!pcl_isfinite(shots->points[k].descriptor[i])) + NaNs++; + } + + if (NaNs == 0) + { + for (int i = 0; i < size_feat; i++) + { + signatures->points[good].histogram[i] = shots->points[k].descriptor[i]; + } + + good++; + } + } + + signatures->resize (good); + signatures->width = good; + + return true; + + } + + }; + } +} + +#endif /* REC_FRAMEWORK_SHOT_LOCAL_ESTIMATOR_OMP_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h new file mode 100644 index 00000000..3bef98a6 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h @@ -0,0 +1,300 @@ +/* + * normal_estimator.h + * + * Created on: Mar 22, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_NORMAL_ESTIMATOR_H_ +#define REC_FRAMEWORK_NORMAL_ESTIMATOR_H_ + +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + template + class PreProcessorAndNormalEstimator + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + + float + computeMeshResolution (PointInTPtr & input) + { + typedef typename pcl::KdTree::Ptr KdTreeInPtr; + KdTreeInPtr tree = boost::make_shared > (false); + tree->setInputCloud (input); + + std::vector nn_indices (9); + std::vector nn_distances (9); + std::vector src_indices; + + float sum_distances = 0.0; + std::vector avg_distances (input->points.size ()); + // Iterate through the source data set + for (size_t i = 0; i < input->points.size (); ++i) + { + tree->nearestKSearch (input->points[i], 9, nn_indices, nn_distances); + + float avg_dist_neighbours = 0.0; + for (size_t j = 1; j < nn_indices.size (); j++) + avg_dist_neighbours += sqrtf (nn_distances[j]); + + avg_dist_neighbours /= static_cast (nn_indices.size ()); + + avg_distances[i] = avg_dist_neighbours; + sum_distances += avg_dist_neighbours; + } + + std::sort (avg_distances.begin (), avg_distances.end ()); + float avg = avg_distances[static_cast (avg_distances.size ()) / 2 + 1]; + return avg; + } + + public: + + bool compute_mesh_resolution_; + bool do_voxel_grid_; + bool remove_outliers_; + + //this values are used when CMR=false + float grid_resolution_; + float normal_radius_; + + //this are used when CMR=true + float factor_normals_; + float factor_voxel_grid_; + float mesh_resolution_; + + PreProcessorAndNormalEstimator () + { + remove_outliers_ = do_voxel_grid_ = compute_mesh_resolution_ = false; + } + + void + setFactorsForCMR (float f1, float f2) + { + factor_voxel_grid_ = f1; + factor_normals_ = f2; + } + + void + setValuesForCMRFalse (float f1, float f2) + { + grid_resolution_ = f1; + normal_radius_ = f2; + } + + void + setDoVoxelGrid (bool b) + { + do_voxel_grid_ = b; + } + + void + setRemoveOutliers (bool b) + { + remove_outliers_ = b; + } + + void + setCMR (bool b) + { + compute_mesh_resolution_ = b; + } + + void + estimate (PointInTPtr & in, PointInTPtr & out, pcl::PointCloud::Ptr & normals) + { + if (compute_mesh_resolution_) + { + mesh_resolution_ = computeMeshResolution (in); + std::cout << "compute mesh resolution:" << mesh_resolution_ << std::endl; + } + + if (do_voxel_grid_) + { + pcl::ScopeTime t ("Voxel grid..."); + float voxel_grid_size = grid_resolution_; + if (compute_mesh_resolution_) + { + voxel_grid_size = mesh_resolution_ * factor_voxel_grid_; + } + + pcl::VoxelGrid grid_; + grid_.setInputCloud (in); + grid_.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size); + grid_.setDownsampleAllData (true); + grid_.filter (*out); + } + else + { + out = in; + } + + if (out->points.size () == 0) + { + PCL_WARN("NORMAL estimator: Cloud has no points after voxel grid, wont be able to compute normals!\n"); + return; + } + + if (remove_outliers_) + { + pcl::ScopeTime t ("remove_outliers_..."); + PointInTPtr out2 (new pcl::PointCloud ()); + float radius = normal_radius_; + if (compute_mesh_resolution_) + { + radius = mesh_resolution_ * factor_normals_; + if (do_voxel_grid_) + radius *= factor_voxel_grid_; + } + + //in synthetic views the render grazes some parts of the objects + //thus creating a very sparse set of points that causes the normals to be very noisy + //remove these points + pcl::RadiusOutlierRemoval sor; + sor.setInputCloud (out); + sor.setRadiusSearch (radius); + sor.setMinNeighborsInRadius (16); + sor.filter (*out2); + out = out2; + + } + + if (out->points.size () == 0) + { + PCL_WARN("NORMAL estimator: Cloud has no points after removing outliers...!\n"); + return; + } + + float radius = normal_radius_; + if (compute_mesh_resolution_) + { + radius = mesh_resolution_ * factor_normals_; + if (do_voxel_grid_) + radius *= factor_voxel_grid_; + } + + if (out->isOrganized ()) + { + typedef typename pcl::IntegralImageNormalEstimation NormalEstimator_; + NormalEstimator_ n3d; + n3d.setNormalEstimationMethod (n3d.COVARIANCE_MATRIX); + //n3d.setNormalEstimationMethod (n3d.AVERAGE_3D_GRADIENT); + n3d.setInputCloud (out); + n3d.setRadiusSearch (radius); + n3d.setKSearch (0); + //n3d.setMaxDepthChangeFactor(0.02f); + //n3d.setNormalSmoothingSize(15.0f); + { + pcl::ScopeTime t ("compute normals..."); + n3d.compute (*normals); + } + } + else + { + + //check nans before computing normals + { + pcl::ScopeTime t ("check nans..."); + int j = 0; + for (size_t i = 0; i < out->points.size (); ++i) + { + if (!pcl_isfinite (out->points[i].x) || !pcl_isfinite (out->points[i].y) || !pcl_isfinite (out->points[i].z)) + continue; + + out->points[j] = out->points[i]; + j++; + } + + if (j != static_cast (out->points.size ())) + { + PCL_ERROR("Contain nans..."); + } + + out->points.resize (j); + out->width = j; + out->height = 1; + } + + typedef typename pcl::NormalEstimation NormalEstimator_; + NormalEstimator_ n3d; + typename pcl::search::KdTree::Ptr normals_tree (new pcl::search::KdTree); + normals_tree->setInputCloud (out); + n3d.setRadiusSearch (radius); + n3d.setSearchMethod (normals_tree); + n3d.setInputCloud (out); + { + pcl::ScopeTime t ("compute normals not organized..."); + n3d.compute (*normals); + } + } + + //check nans... + if (!out->isOrganized ()) + { + pcl::ScopeTime t ("check nans..."); + int j = 0; + for (size_t i = 0; i < normals->points.size (); ++i) + { + if (!pcl_isfinite (normals->points[i].normal_x) || !pcl_isfinite (normals->points[i].normal_y) + || !pcl_isfinite (normals->points[i].normal_z)) + continue; + + normals->points[j] = normals->points[i]; + out->points[j] = out->points[i]; + j++; + } + + normals->points.resize (j); + normals->width = j; + normals->height = 1; + + out->points.resize (j); + out->width = j; + out->height = 1; + } + else + { + //is is organized, we set the xyz points to NaN + pcl::ScopeTime t ("check nans organized..."); + bool NaNs = false; + for (size_t i = 0; i < normals->points.size (); ++i) + { + if (pcl_isfinite (normals->points[i].normal_x) && pcl_isfinite (normals->points[i].normal_y) + && pcl_isfinite (normals->points[i].normal_z)) + continue; + + NaNs = true; + + out->points[i].x = out->points[i].y = out->points[i].z = std::numeric_limits::quiet_NaN (); + } + + if (NaNs) + { + PCL_WARN("normals contain NaNs\n"); + out->is_dense = false; + } + } + + /*for (size_t i = 0; i < out->points.size (); i++) + { + int r, g, b; + r = static_cast (out->points[i].r); + g = static_cast (out->points[i].g); + b = static_cast (out->points[i].b); + std::cout << "in normal estimator:" << r << " " << g << " " << b << std::endl; + }*/ + } + }; + } +} + +#endif /* REC_FRAMEWORK_NORMAL_ESTIMATOR_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h new file mode 100644 index 00000000..3af9d17c --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/mesh_source.h @@ -0,0 +1,294 @@ +/* + * ply_source.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_MESH_SOURCE_H_ +#define REC_FRAMEWORK_MESH_SOURCE_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + /** + * \brief Data source class based on mesh models + * \author Aitor Aldoma + */ + + template + class MeshSource : public Source + { + typedef Source SourceT; + typedef Model ModelT; + + using SourceT::path_; + using SourceT::models_; + using SourceT::createTrainingDir; + using SourceT::getModelsInDirectory; + using SourceT::model_scale_; + + int tes_level_; + int resolution_; + float radius_sphere_; + float view_angle_; + bool gen_organized_; + boost::function campos_constraints_func_; + + public: + + using SourceT::setFilterDuplicateViews; + + MeshSource () : + SourceT () + { + gen_organized_ = false; + } + + void + setTesselationLevel (int lev) + { + tes_level_ = lev; + } + + void + setCamPosConstraints (boost::function & bb) + { + campos_constraints_func_ = bb; + } + + void + setResolution (int res) + { + resolution_ = res; + } + + void + setRadiusSphere (float r) + { + radius_sphere_ = r; + } + + void + setViewAngle (float a) + { + view_angle_ = a; + } + + void + loadOrGenerate (std::string & dir, std::string & model_path, ModelT & model) + { + std::stringstream pathmodel; + pathmodel << dir << "/" << model.class_ << "/" << model.id_; + bf::path trained_dir = pathmodel.str (); + + model.views_.reset (new std::vector::Ptr>); + model.poses_.reset (new std::vector >); + model.self_occlusions_.reset (new std::vector); + model.assembled_.reset (new pcl::PointCloud); + uniform_sampling (model_path, 100000, *model.assembled_, model_scale_); + + if (bf::exists (trained_dir)) + { + //load views, poses and self-occlusions + std::vector < std::string > view_filenames; + int number_of_views = 0; + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (trained_dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (!(bf::is_directory (*itr))) + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; + std::vector < std::string > strs_; + +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ()).filename (); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + boost::split (strs_, file, boost::is_any_of ("_")); + + std::string extension = strs[strs.size () - 1]; + + if (extension == "pcd" && strs_[0] == "view") + { +#if BOOST_FILESYSTEM_VERSION == 3 + view_filenames.push_back ((itr->path ().filename ()).string()); +#else + view_filenames.push_back ((itr->path ()).filename ()); +#endif + + number_of_views++; + } + } + } + + for (size_t i = 0; i < view_filenames.size (); i++) + { + std::stringstream view_file; + view_file << pathmodel.str () << "/" << view_filenames[i]; + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::io::loadPCDFile (view_file.str (), *cloud); + + model.views_->push_back (cloud); + + std::string file_replaced1 (view_filenames[i]); + boost::replace_all (file_replaced1, "view", "pose"); + boost::replace_all (file_replaced1, ".pcd", ".txt"); + + std::string file_replaced2 (view_filenames[i]); + boost::replace_all (file_replaced2, "view", "entropy"); + boost::replace_all (file_replaced2, ".pcd", ".txt"); + + //read pose as well + std::stringstream pose_file; + pose_file << pathmodel.str () << "/" << file_replaced1; + + Eigen::Matrix4f pose; + PersistenceUtils::readMatrixFromFile (pose_file.str (), pose); + + model.poses_->push_back (pose); + + //read entropy as well + std::stringstream entropy_file; + entropy_file << pathmodel.str () << "/" << file_replaced2; + float entropy = 0; + PersistenceUtils::readFloatFromFile (entropy_file.str (), entropy); + model.self_occlusions_->push_back (entropy); + + } + + } + else + { + //load PLY model and scale it + vtkSmartPointer reader = vtkSmartPointer::New (); + reader->SetFileName (model_path.c_str ()); + + vtkSmartPointer trans = vtkSmartPointer::New (); + trans->Scale (model_scale_, model_scale_, model_scale_); + trans->Modified (); + trans->Update (); + + vtkSmartPointer filter_scale = vtkSmartPointer::New (); + filter_scale->SetTransform (trans); + filter_scale->SetInputConnection (reader->GetOutputPort ()); + filter_scale->Update (); + + vtkSmartPointer mapper = filter_scale->GetOutput (); + + //generate views + pcl::apps::RenderViewsTesselatedSphere render_views; + render_views.setResolution (resolution_); + render_views.setUseVertices (false); + render_views.setRadiusSphere (radius_sphere_); + render_views.setComputeEntropies (true); + render_views.setTesselationLevel (tes_level_); + render_views.setViewAngle (view_angle_); + render_views.addModelFromPolyData (mapper); + render_views.setGenOrganized (gen_organized_); + render_views.setCamPosConstraints (campos_constraints_func_); + render_views.generateViews (); + + std::vector::Ptr> views_xyz_orig; + std::vector < Eigen::Matrix4f, Eigen::aligned_allocator > poses; + std::vector entropies; + + render_views.getViews (views_xyz_orig); + render_views.getPoses (poses); + render_views.getEntropies (entropies); + + model.views_.reset (new std::vector::Ptr> ()); + model.poses_.reset (new std::vector > ()); + model.self_occlusions_.reset (new std::vector ()); + + for (size_t i = 0; i < views_xyz_orig.size (); i++) + { + model.views_->push_back (views_xyz_orig[i]); + model.poses_->push_back (poses[i]); + model.self_occlusions_->push_back (entropies[i]); + } + + std::stringstream direc; + direc << dir << "/" << model.class_ << "/" << model.id_; + this->createClassAndModelDirectories (dir, model.class_, model.id_); + + for (size_t i = 0; i < model.views_->size (); i++) + { + //save generated model for future use + std::stringstream path_view; + path_view << direc.str () << "/view_" << i << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *(model.views_->at (i))); + + std::stringstream path_pose; + path_pose << direc.str () << "/pose_" << i << ".txt"; + + pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile (path_pose.str (), model.poses_->at (i)); + + std::stringstream path_entropy; + path_entropy << direc.str () << "/entropy_" << i << ".txt"; + pcl::rec_3d_framework::PersistenceUtils::writeFloatToFile (path_entropy.str (), model.self_occlusions_->at (i)); + } + + loadOrGenerate (dir, model_path, model); + + } + } + + /** + * \brief Creates the model representation of the training set, generating views if needed + */ + void + generate (std::string & training_dir) + { + + //create training dir fs if not existent + createTrainingDir (training_dir); + + //get models in directory + std::vector < std::string > files; + std::string start = ""; + std::string ext = std::string ("ply"); + bf::path dir = path_; + getModelsInDirectory (dir, start, files, ext); + + models_.reset (new std::vector); + + for (size_t i = 0; i < files.size (); i++) + { + ModelT m; + this->getIdAndClassFromFilename (files[i], m.id_, m.class_); + + //check which of them have been trained using training_dir and the model_id_ + //load views, poses and self-occlusions for those that exist + //generate otherwise + std::cout << files[i] << std::endl; + std::stringstream model_path; + model_path << path_ << "/" << files[i]; + std::string path_model = model_path.str (); + loadOrGenerate (training_dir, path_model, m); + + models_->push_back (m); + } + } + }; + } +} + +#endif /* REC_FRAMEWORK_MESH_SOURCE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h new file mode 100644 index 00000000..f6759c0d --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/registered_views_source.h @@ -0,0 +1,388 @@ +/* + * ply_source.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_MESH_SOURCE_H_ +#define REC_FRAMEWORK_MESH_SOURCE_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + /** + * \brief Data source class based on partial views from sensor. + * In this case, the training data is obtained directly from a depth sensor. + * The filesystem should contain pcd files (representing a view of an object in + * camera coordinates) and each view needs to be associated with a txt file + * containing a 4x4 matrix representing the transformation from camera coordinates + * to a global object coordinates frame. + * \author Aitor Aldoma + */ + + template + class RegisteredViewsSource : public Source + { + typedef Source SourceT; + typedef Model ModelT; + + using SourceT::path_; + using SourceT::models_; + using SourceT::createTrainingDir; + using SourceT::getModelsInDirectory; + using SourceT::model_scale_; + + std::string view_prefix_; + int pose_files_order_; //0 is row, 1 is column + + public: + RegisteredViewsSource () + { + view_prefix_ = std::string ("view"); + pose_files_order_ = 0; + } + + void + setPrefix (std::string & pre) + { + view_prefix_ = pre; + } + + void + setPoseRowOrder (int o) + { + pose_files_order_ = o; + } + + void + getViewsFilenames (bf::path & path_with_views, std::vector & view_filenames) + { + int number_of_views = 0; + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (path_with_views); itr != end_itr; ++itr) + { + if (!(bf::is_directory (*itr))) + { + std::vector < std::string > strs; + std::vector < std::string > strs_; + +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ()).filename (); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + boost::split (strs_, file, boost::is_any_of ("_")); + + std::string extension = strs[strs.size () - 1]; + + if (extension == "pcd" && (strs_[0].compare (view_prefix_) == 0)) + { +#if BOOST_FILESYSTEM_VERSION == 3 + view_filenames.push_back ((itr->path ().filename ()).string()); +#else + view_filenames.push_back ((itr->path ()).filename ()); +#endif + + number_of_views++; + } + } + } + } + + void + assembleModelFromViewsAndPoses(ModelT & model, std::vector > & poses) { + for(size_t i=0; i < model.views_->size(); i++) { + Eigen::Matrix4f inv = poses[i]; + typename pcl::PointCloud::Ptr global_cloud(new pcl::PointCloud); + pcl::transformPointCloud(*(model.views_->at(i)),*global_cloud, inv); + *(model.assembled_) += *global_cloud; + } + } + + void + loadOrGenerate (std::string & dir, std::string & model_path, ModelT & model) + { + std::stringstream pathmodel; + pathmodel << dir << "/" << model.class_ << "/" << model.id_; + bf::path trained_dir = pathmodel.str (); + + model.views_.reset (new std::vector::Ptr>); + model.poses_.reset (new std::vector >); + model.self_occlusions_.reset (new std::vector); + + if (bf::exists (trained_dir)) + { + //load views and poses + std::vector < std::string > view_filenames; + int number_of_views = 0; + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (trained_dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (!(bf::is_directory (*itr))) + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; + std::vector < std::string > strs_; + +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ()).filename (); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + boost::split (strs_, file, boost::is_any_of ("_")); + + std::string extension = strs[strs.size () - 1]; + + if (extension == "pcd" && strs_[0] == "view") + { +#if BOOST_FILESYSTEM_VERSION == 3 + view_filenames.push_back ((itr->path ().filename ()).string()); +#else + view_filenames.push_back ((itr->path ()).filename ()); +#endif + + number_of_views++; + } + } + } + + std::vector > poses_to_assemble_; + + for (size_t i = 0; i < view_filenames.size (); i++) + { + std::stringstream view_file; + view_file << pathmodel.str () << "/" << view_filenames[i]; + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::io::loadPCDFile (view_file.str (), *cloud); + + model.views_->push_back (cloud); + + std::string file_replaced1 (view_filenames[i]); + boost::replace_all (file_replaced1, "view", "pose"); + boost::replace_all (file_replaced1, ".pcd", ".txt"); + + //read pose as well + std::stringstream pose_file; + pose_file << pathmodel.str () << "/" << file_replaced1; + Eigen::Matrix4f pose; + PersistenceUtils::readMatrixFromFile (pose_file.str (), pose); + + if(pose_files_order_ != 0) { + //std::cout << "Transpose..." << std::endl; + + Eigen::Matrix4f pose_trans = pose.transpose(); + poses_to_assemble_.push_back(pose_trans); + //pose = pose_trans; + //std::cout << pose << std::endl; + } + + //std::cout << "pose being push backed to model" << std::endl; + std::cout << pose << std::endl; + + //the recognizer assumes transformation from M to CC + Eigen::Matrix4f inv = pose.inverse(); + model.poses_->push_back (inv); + + model.self_occlusions_->push_back (-1.f); + + } + + model.assembled_.reset (new pcl::PointCloud); + assembleModelFromViewsAndPoses(model, poses_to_assemble_); + + /*pcl::visualization::PCLVisualizer vis ("results"); + pcl::visualization::PointCloudColorHandlerCustom random_handler (model.assembled_, 255, 0, 0); + vis.addPointCloud (model.assembled_, random_handler, "points"); + + Eigen::Matrix4f view_transformation = model.poses_->at(0).inverse(); + typename pcl::PointCloud::Ptr view_trans(new pcl::PointCloud); + pcl::transformPointCloud(*(model.views_->at(0)), *view_trans, view_transformation); + + pcl::visualization::PointCloudColorHandlerCustom random_handler2 (view_trans, 0, 255, 0); + vis.addPointCloud (view_trans, random_handler2, "view"); + + vis.addCoordinateSystem(0.1); + vis.spin ();*/ + + } + else + { + + //we just need to copy the views to the training directory + std::stringstream direc; + direc << dir << "/" << model.class_ << "/" << model.id_; + createClassAndModelDirectories (dir, model.class_, model.id_); + + std::vector < std::string > view_filenames; + bf::path model_dir = model_path; + + getViewsFilenames (model_dir, view_filenames); + std::cout << view_filenames.size () << std::endl; + + for (size_t i = 0; i < view_filenames.size (); i++) + { + std::stringstream view_file; + view_file << model_path << "/" << view_filenames[i]; + typename pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); + pcl::io::loadPCDFile (view_file.str (), *cloud); + + std::cout << view_file.str () << std::endl; + + std::stringstream path_view; + path_view << direc.str () << "/view_" << i << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *cloud); + + std::string file_replaced1 (view_file.str ()); + boost::replace_all (file_replaced1, view_prefix_, "pose"); + boost::replace_all (file_replaced1, ".pcd", ".txt"); + + Eigen::Matrix4f pose; + PersistenceUtils::readMatrixFromFile (file_replaced1, pose); + + std::cout << pose << std::endl; + + if(pose_files_order_ == 0) { + std::cout << "Transpose..." << std::endl; + Eigen::Matrix4f pose_trans = pose.transpose(); + pose = pose_trans; + std::cout << pose << std::endl; + } + + std::stringstream path_pose; + path_pose << direc.str () << "/pose_" << i << ".txt"; + pcl::rec_3d_framework::PersistenceUtils::writeMatrixToFile (path_pose.str (), pose); + } + + loadOrGenerate (dir, model_path, model); + + } + } + + bool + isleafDirectory (bf::path & path) + { + bf::directory_iterator end_itr; + bool no_dirs_inside = true; + for (bf::directory_iterator itr (path); itr != end_itr; ++itr) + { + if (bf::is_directory (*itr)) + { + no_dirs_inside = false; + } + } + + return no_dirs_inside; + } + + void + getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector & relative_paths) + { + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (bf::is_directory (*itr)) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/"; +#else + std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/"; +#endif + + bf::path curr_path = itr->path (); + + if (isleafDirectory (curr_path)) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string path = rel_path_so_far + (itr->path ().filename ()).string(); +#else + std::string path = rel_path_so_far + (itr->path ()).filename (); +#endif + relative_paths.push_back (path); + + } + else + { + getModelsInDirectory (curr_path, so_far, relative_paths); + } + } + } + } + + /** + * \brief Creates the model representation of the training set, generating views if needed + */ + void + generate (std::string & training_dir) + { + + //create training dir fs if not existent + createTrainingDir (training_dir); + + //get models in directory + std::vector < std::string > files; + std::string start = ""; + bf::path dir = path_; + getModelsInDirectory (dir, start, files); + + models_.reset (new std::vector); + + for (size_t i = 0; i < files.size (); i++) + { + ModelT m; + + std::vector < std::string > strs; + boost::split (strs, files[i], boost::is_any_of ("/\\")); + std::string name = strs[strs.size () - 1]; + + if (strs.size () == 1) + { + m.id_ = strs[0]; + } + else + { + std::stringstream ss; + for (int i = 0; i < (static_cast (strs.size ()) - 1); i++) + { + ss << strs[i]; + if (i != (static_cast (strs.size ()) - 1)) + ss << "/"; + } + + m.class_ = ss.str (); + m.id_ = strs[strs.size () - 1]; + } + + std::cout << m.class_ << " . " << m.id_ << std::endl; + //check which of them have been trained using training_dir and the model_id_ + //load views, poses and self-occlusions for those that exist + //generate otherwise + + std::stringstream model_path; + model_path << path_ << "/" << files[i]; + std::string path_model = model_path.str (); + loadOrGenerate (training_dir, path_model, m); + + models_->push_back (m); + + //std::cout << files[i] << std::endl; + } + } + }; + } +} + +#endif /* REC_FRAMEWORK_MESH_SOURCE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h new file mode 100644 index 00000000..d5706085 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h @@ -0,0 +1,301 @@ +/* + * source.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_VIEWS_SOURCE_H_ +#define REC_FRAMEWORK_VIEWS_SOURCE_H_ + +#include +#include +#include +#include +#include +#include + +namespace bf = boost::filesystem; + +namespace pcl +{ + namespace rec_3d_framework + { + + /** + * \brief Model representation + * \author Aitor Aldoma + */ + + template + class Model + { + typedef typename pcl::PointCloud::Ptr PointTPtr; + typedef typename pcl::PointCloud::ConstPtr PointTPtrConst; + + public: + boost::shared_ptr > views_; + boost::shared_ptr > > poses_; + boost::shared_ptr > self_occlusions_; + std::string id_; + std::string class_; + PointTPtr assembled_; + typename std::map voxelized_assembled_; + + bool + operator== (const Model &other) const + { + return (id_ == other.id_) && (class_ == other.class_); + } + + PointTPtrConst + getAssembled (float resolution) + { + if(resolution <= 0) + return assembled_; + + typename std::map::iterator it = voxelized_assembled_.find (resolution); + if (it == voxelized_assembled_.end ()) + { + PointTPtr voxelized (new pcl::PointCloud); + pcl::VoxelGrid grid_; + grid_.setInputCloud (assembled_); + grid_.setLeafSize (resolution, resolution, resolution); + grid_.setDownsampleAllData(true); + grid_.filter (*voxelized); + + PointTPtrConst voxelized_const (new pcl::PointCloud (*voxelized)); + voxelized_assembled_[resolution] = voxelized_const; + return voxelized_const; + } + + return it->second; + } + }; + + /** + * \brief Abstract data source class, manages filesystem, incremental training, etc. + * \author Aitor Aldoma + */ + + template + class Source + { + + protected: + typedef Model ModelT; + std::string path_; + boost::shared_ptr > models_; + float model_scale_; + bool filter_duplicate_views_; + bool load_views_; + + void + getIdAndClassFromFilename (std::string & filename, std::string & id, std::string & classname) + { + + std::vector < std::string > strs; + boost::split (strs, filename, boost::is_any_of ("/\\")); + std::string name = strs[strs.size () - 1]; + + std::stringstream ss; + for (int i = 0; i < (static_cast (strs.size ()) - 1); i++) + { + ss << strs[i]; + if (i != (static_cast (strs.size ()) - 1)) + ss << "/"; + } + + classname = ss.str (); + id = name.substr (0, name.length () - 4); + } + + void + createTrainingDir (std::string & training_dir) + { + bf::path trained_dir = training_dir; + if (!bf::exists (trained_dir)) + bf::create_directory (trained_dir); + } + + void + createClassAndModelDirectories (std::string & training_dir, std::string & class_str, std::string & id_str) + { + std::vector < std::string > strs; + boost::split (strs, class_str, boost::is_any_of ("/\\")); + + std::stringstream ss; + ss << training_dir << "/"; + for (size_t i = 0; i < strs.size (); i++) + { + ss << strs[i] << "/"; + bf::path trained_dir = ss.str (); + if (!bf::exists (trained_dir)) + bf::create_directory (trained_dir); + } + + ss << id_str; + bf::path trained_dir = ss.str (); + if (!bf::exists (trained_dir)) + bf::create_directory (trained_dir); + } + + public: + + Source() { + load_views_ = true; + } + + float + getScale () + { + return model_scale_; + } + + void + setModelScale (float s) + { + model_scale_ = s; + } + + void setFilterDuplicateViews(bool f) { + filter_duplicate_views_ = f; + std::cout << "setting filter duplicate views to " << f << std::endl; + } + + void + getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector & relative_paths, std::string & ext) + { + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (bf::is_directory (*itr)) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/"; +#else + std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/"; +#endif + + bf::path curr_path = itr->path (); + getModelsInDirectory (curr_path, so_far, relative_paths, ext); + } + else + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ()).filename (); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + std::string extension = strs[strs.size () - 1]; + + if (extension.compare (ext) == 0) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string path = rel_path_so_far + (itr->path ().filename ()).string(); +#else + std::string path = rel_path_so_far + (itr->path ()).filename (); +#endif + + relative_paths.push_back (path); + } + } + } + } + + void + voxelizeAllModels (float resolution) + { + for (size_t i = 0; i < models_->size (); i++) + { + models_->at (i).getAssembled (resolution); + } + } + + /** + * \brief Generate model representation + */ + virtual void + generate (std::string & training_dir)=0; + + /** + * \brief Get the generated model + */ + boost::shared_ptr > + getModels () + { + return models_; + } + + boost::shared_ptr > + getModels (std::string & model_id) + { + + typename std::vector::iterator it = models_->begin (); + while (it != models_->end ()) + { + if (model_id.compare ((*it).id_) != 0) + { + it = models_->erase (it); + } + else + { + it++; + } + } + + return models_; + } + + bool + modelAlreadyTrained (ModelT m, std::string & base_dir, std::string & descr_name) + { + std::stringstream dir; + dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name; + bf::path desc_dir = dir.str (); + std::cout << dir.str () << std::endl; + if (bf::exists (desc_dir)) + { + return true; + } + + return false; + } + + std::string + getModelDescriptorDir (ModelT m, std::string & base_dir, std::string & descr_name) + { + std::stringstream dir; + dir << base_dir << "/" << m.class_ << "/" << m.id_ << "/" << descr_name; + return dir.str (); + } + + void + removeDescDirectory (ModelT m, std::string & base_dir, std::string & descr_name) + { + std::string dir = getModelDescriptorDir (m, base_dir, descr_name); + + bf::path desc_dir = dir; + if (bf::exists (desc_dir)) + bf::remove_all (desc_dir); + } + + void + setPath (std::string & path) + { + path_ = path; + } + + void setLoadViews(bool load) { + load_views_ = load; + } + }; + } +} + +#endif /* REC_FRAMEWORK_VIEWS_SOURCE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h new file mode 100644 index 00000000..5a78c51d --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h @@ -0,0 +1,223 @@ +/* + * global.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_GLOBAL_PIPELINE_H_ +#define REC_FRAMEWORK_GLOBAL_PIPELINE_H_ + +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + template + class PCL_EXPORTS GlobalClassifier { + public: + typedef typename pcl::PointCloud::Ptr PointInTPtr; + + virtual void + setNN (int nn) = 0; + + virtual void + getCategory (std::vector & categories) = 0; + + virtual void + getConfidence (std::vector & conf) = 0; + + virtual void + classify () = 0; + + virtual void + setIndices (std::vector & indices) = 0; + + virtual void + setInputCloud (const PointInTPtr & cloud) = 0; + }; + + /** + * \brief Nearest neighbor search based classification of PCL point type features. + * FLANN is used to identify a neighborhood, based on which different scoring schemes + * can be employed to obtain likelihood values for a specified list of classes. + * Available features: ESF, VFH, CVFH + * See apps/3d_rec_framework/tools/apps/global_classification.cpp for usage + * \author Aitor Aldoma + */ + + template class Distance, typename PointInT, typename FeatureT> + class PCL_EXPORTS GlobalNNPipeline : public pcl::rec_3d_framework::GlobalClassifier + { + + protected: + + struct index_score + { + int idx_models_; + int idx_input_; + double score_; + }; + + struct sortIndexScores + { + bool + operator() (const index_score& d1, const index_score& d2) + { + return d1.score_ < d2.score_; + } + } sortIndexScoresOp; + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef Distance DistT; + typedef Model ModelT; + + /** \brief Directory where the trained structure will be saved */ + std::string training_dir_; + + /** \brief Point cloud to be classified */ + PointInTPtr input_; + + /** \brief Model data source */ + typename boost::shared_ptr > source_; + + /** \brief Computes a feature */ + typename boost::shared_ptr > estimator_; + + /** \brief Descriptor name */ + std::string descr_name_; + + typedef std::pair > flann_model; + flann::Matrix flann_data_; + flann::Index * flann_index_; + std::vector flann_models_; + + std::vector indices_; + + //load features from disk and create flann structure + void + loadFeaturesAndCreateFLANN (); + + inline void + convertToFLANN (const std::vector &models, flann::Matrix &data) + { + data.rows = models.size (); + data.cols = models[0].second.size (); // number of histogram bins + + flann::Matrix flann_data (new float[models.size () * models[0].second.size ()], models.size (), models[0].second.size ()); + + for (size_t i = 0; i < data.rows; ++i) + for (size_t j = 0; j < data.cols; ++j) + { + flann_data.ptr ()[i * data.cols + j] = models[i].second[j]; + } + + data = flann_data; + } + + void + nearestKSearch (flann::Index * index, const flann_model &model, int k, flann::Matrix &indices, flann::Matrix &distances); + + int NN_; + std::vector categories_; + std::vector confidences_; + + std::string first_nn_category_; + + public: + + GlobalNNPipeline () + { + NN_ = 1; + } + + ~GlobalNNPipeline () + { + } + + void + setNN (int nn) + { + NN_ = nn; + } + + void + getCategory (std::vector & categories) + { + categories = categories_; + } + + void + getConfidence (std::vector & conf) + { + conf = confidences_; + } + + /** + * \brief Initializes the FLANN structure from the provided source + */ + + void + initialize (bool force_retrain = false); + + /** + * \brief Performs classification + */ + + void + classify (); + + /** + * \brief Sets the model data source_ + */ + void + setDataSource (typename boost::shared_ptr > & source) + { + source_ = source; + } + + /** + * \brief Sets the model data source_ + */ + + void + setFeatureEstimator (typename boost::shared_ptr > & feat) + { + estimator_ = feat; + } + + void + setIndices (std::vector & indices) + { + indices_ = indices; + } + + /** + * \brief Sets the input cloud to be classified + */ + void + setInputCloud (const PointInTPtr & cloud) + { + input_ = cloud; + } + + void + setDescriptorName (std::string & name) + { + descr_name_ = name; + } + + void + setTrainingDir (std::string & dir) + { + training_dir_ = dir; + } + }; + } +} +#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h new file mode 100644 index 00000000..93202b78 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h @@ -0,0 +1,271 @@ +/* + * global.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_GLOBAL_RECOGNIZER_CRH_H_ +#define REC_FRAMEWORK_GLOBAL_RECOGNIZER_CRH_H_ + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + /** + * \brief Nearest neighbor search based classification of PCL point type features. + * FLANN is used to identify a neighborhood, based on which different scoring schemes + * can be employed to obtain likelihood values for a specified list of classes. + * Available features: ESF, VFH, CVFH + * \author Aitor Aldoma + */ + + template class Distance, typename PointInT, typename FeatureT> + class PCL_EXPORTS GlobalNNCRHRecognizer + { + + protected: + + struct index_score + { + int idx_models_; + int idx_input_; + double score_; + }; + + struct sortIndexScores + { + bool + operator() (const index_score& d1, const index_score& d2) + { + return d1.score_ < d2.score_; + } + } sortIndexScoresOp; + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::ConstPtr ConstPointInTPtr; + + typedef Distance DistT; + typedef Model ModelT; + typedef pcl::PointCloud > CRHPointCloud; + + /** \brief Directory where the trained structure will be saved */ + std::string training_dir_; + + /** \brief Point cloud to be classified */ + PointInTPtr input_; + + /** \brief Model data source */ + typename boost::shared_ptr > source_; + + /** \brief Computes a feature */ + typename boost::shared_ptr > crh_estimator_; + + /** \brief Hypotheses verification algorithm */ + typename boost::shared_ptr > hv_algorithm_; + + /** \brief Descriptor name */ + std::string descr_name_; + + int ICP_iterations_; + + bool noisify_; + float noise_; + + class flann_model + { + public: + ModelT model; + int view_id; + int descriptor_id; + std::vector descr; + }; + + flann::Matrix flann_data_; + flann::Index * flann_index_; + std::vector flann_models_; + + + bool use_cache_; + std::map, Eigen::Matrix4f, std::less >, Eigen::aligned_allocator, Eigen::Matrix4f> > > poses_cache_; + std::map, Eigen::Vector3f > centroids_cache_; + + std::vector indices_; + + //load features from disk and create flann structure + void + loadFeaturesAndCreateFLANN (); + + inline void + convertToFLANN (const std::vector &models, flann::Matrix &data) + { + data.rows = models.size (); + data.cols = models[0].descr.size (); // number of histogram bins + + flann::Matrix flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ()); + + for (size_t i = 0; i < data.rows; ++i) + for (size_t j = 0; j < data.cols; ++j) + { + flann_data.ptr ()[i * data.cols + j] = models[i].descr[j]; + } + + data = flann_data; + } + + void + nearestKSearch (flann::Index * index, const flann_model &model, int k, flann::Matrix &indices, flann::Matrix &distances); + + void + getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix); + + void + getCRH (ModelT & model, int view_id, int d_id, CRHPointCloud::Ptr & hist); + + void + getCentroid (ModelT & model, int view_id, int d_id, Eigen::Vector3f & centroid); + + void + getView (ModelT & model, int view_id, PointInTPtr & view); + + int NN_; + + boost::shared_ptr > models_; + boost::shared_ptr > > transforms_; + + public: + + GlobalNNCRHRecognizer () + { + ICP_iterations_ = 0; + noisify_ = false; + do_CRH_ = true; + } + + ~GlobalNNCRHRecognizer () + { + } + + void setNoise(float n) { + noisify_ = true; + noise_ = n; + } + + void setDOCRH(bool b) { + do_CRH_ = b; + } + + void + setNN (int nn) + { + NN_ = nn; + } + + void + setICPIterations (int it) + { + ICP_iterations_ = it; + } + + /** + * \brief Initializes the FLANN structure from the provided source + */ + + void + initialize (bool force_retrain = false); + + /** + * \brief Sets the model data source_ + */ + void + setDataSource (typename boost::shared_ptr > & source) + { + source_ = source; + } + + /** + * \brief Sets the model data source_ + */ + + void + setFeatureEstimator (typename boost::shared_ptr > & feat) + { + crh_estimator_ = feat; + } + + /** + * \brief Sets the HV algorithm + */ + void + setHVAlgorithm (typename boost::shared_ptr > & alg) + { + hv_algorithm_ = alg; + } + + void + setIndices (std::vector & indices) + { + indices_ = indices; + } + + /** + * \brief Sets the input cloud to be classified + */ + void + setInputCloud (const PointInTPtr & cloud) + { + input_ = cloud; + } + + void + setDescriptorName (std::string & name) + { + descr_name_ = name; + } + + void + setTrainingDir (std::string & dir) + { + training_dir_ = dir; + } + + /** + * \brief Performs recognition on the input cloud + */ + + void + recognize (); + + boost::shared_ptr > + getModels () + { + return models_; + } + + boost::shared_ptr > > + getTransforms () + { + return transforms_; + } + + void + setUseCache (bool u) + { + use_cache_ = u; + } + + bool do_CRH_; + + }; + } +} +#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h new file mode 100644 index 00000000..5f070047 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h @@ -0,0 +1,355 @@ +/* + * global.h + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_GLOBAL_RECOGNIZER_CVFH_H_ +#define REC_FRAMEWORK_GLOBAL_RECOGNIZER_CVFH_H_ + +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + /** + * \brief Nearest neighbor search based classification of PCL point type features. + * Available features: CVFH + * \author Aitor Aldoma + */ + + template class Distance, typename PointInT, typename FeatureT = pcl::VFHSignature308> + class PCL_EXPORTS GlobalNNCVFHRecognizer + { + + protected: + + struct index_score + { + int idx_models_; + int idx_input_; + double score_; + }; + + struct sortIndexScores + { + bool + operator() (const index_score& d1, const index_score& d2) + { + return d1.score_ < d2.score_; + } + } sortIndexScoresOp; + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::ConstPtr ConstPointInTPtr; + + typedef Distance DistT; + typedef Model ModelT; + + /** \brief Directory where the trained structure will be saved */ + std::string training_dir_; + + /** \brief Point cloud to be classified */ + PointInTPtr input_; + + /** \brief Model data source */ + typename boost::shared_ptr > source_; + + /** \brief Computes a feature */ + typename boost::shared_ptr > micvfh_estimator_; + + /** \brief Hypotheses verification algorithm */ + typename boost::shared_ptr > hv_algorithm_; + + /** \brief Descriptor name */ + std::string descr_name_; + + int ICP_iterations_; + + bool noisify_; + float noise_; + + class flann_model + { + public: + ModelT model; + int view_id; + int descriptor_id; + std::vector descr; + + bool + operator< (const flann_model &other) const + { + if ((this->model.id_.compare (other.model.id_) < 0)) + { + return true; + } + else + { + + if (this->model.id_.compare (other.model.id_) == 0) + { + //check view id + if ((this->view_id < other.view_id)) + { + return true; + } + else + { + if (this->view_id == other.view_id) + { + if (this->descriptor_id < other.descriptor_id) + { + return true; + } + } + } + } + } + + return false; + } + + bool + operator== (const flann_model &other) const + { + return (model.id_ == other.model.id_) && (view_id == other.view_id) && (descriptor_id == other.descriptor_id); + } + + }; + + flann::Matrix flann_data_; + flann::Index * flann_index_; + std::vector flann_models_; + + std::vector > single_categories_data_; + std::vector *> single_categories_index_; + std::vector > > single_categories_pointers_to_models_; + std::map category_to_vectors_indices_; + std::vector categories_to_be_searched_; + bool use_single_categories_; + + bool use_cache_; + std::map, Eigen::Matrix4f, std::less >, Eigen::aligned_allocator, Eigen::Matrix4f> > > poses_cache_; + std::map, Eigen::Vector3f> centroids_cache_; + + std::vector indices_; + + bool compute_scale_; + + //load features from disk and create flann structure + void + loadFeaturesAndCreateFLANN (); + + inline void + convertToFLANN (const std::vector &models, flann::Matrix &data) + { + data.rows = models.size (); + data.cols = models[0].descr.size (); // number of histogram bins + + flann::Matrix flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ()); + + for (size_t i = 0; i < data.rows; ++i) + for (size_t j = 0; j < data.cols; ++j) + { + flann_data.ptr ()[i * data.cols + j] = models[i].descr[j]; + } + + data = flann_data; + } + + inline void + convertToFLANN (const std::vector &models, boost::shared_ptr > & indices, flann::Matrix &data) + { + data.rows = indices->size (); + data.cols = models[0].descr.size (); // number of histogram bins + + flann::Matrix flann_data(new float[indices->size () * models[0].descr.size ()],indices->size(),models[0].descr.size ()); + + for (size_t i = 0; i < data.rows; ++i) + for (size_t j = 0; j < data.cols; ++j) + { + flann_data.ptr()[i * data.cols + j] = models[indices->at(i)].descr[j]; + } + + data = flann_data; + } + + void + nearestKSearch (flann::Index * index, const flann_model &model, int k, flann::Matrix &indices, flann::Matrix &distances); + + void + getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix); + + bool + getRollPose (ModelT & model, int view_id, int d_id, Eigen::Matrix4f & pose_matrix); + + void + getCentroid (ModelT & model, int view_id, int d_id, Eigen::Vector3f & centroid); + + void + getView (ModelT & model, int view_id, PointInTPtr & view); + + int NN_; + + boost::shared_ptr > models_; + boost::shared_ptr > > transforms_; + + std::vector descriptor_distances_; + + public: + + GlobalNNCVFHRecognizer () + { + ICP_iterations_ = 0; + noisify_ = false; + compute_scale_ = false; + use_single_categories_ = false; + } + + ~GlobalNNCVFHRecognizer () + { + } + + void + getDescriptorDistances (std::vector & ds) + { + ds = descriptor_distances_; + } + + void + setComputeScale (bool d) + { + compute_scale_ = d; + } + + void + setCategoriesToUseForRecognition (std::vector & cats_to_use) + { + categories_to_be_searched_.clear (); + categories_to_be_searched_ = cats_to_use; + } + + void setUseSingleCategories(bool b) { + use_single_categories_ = b; + } + + void + setNoise (float n) + { + noisify_ = true; + noise_ = n; + } + + void + setNN (int nn) + { + NN_ = nn; + } + + void + setICPIterations (int it) + { + ICP_iterations_ = it; + } + + /** + * \brief Initializes the FLANN structure from the provided source + */ + + void + initialize (bool force_retrain = false); + + /** + * \brief Sets the model data source_ + */ + void + setDataSource (typename boost::shared_ptr > & source) + { + source_ = source; + } + + /** + * \brief Sets the model data source_ + */ + + void + setFeatureEstimator (typename boost::shared_ptr > & feat) + { + micvfh_estimator_ = feat; + } + + /** + * \brief Sets the HV algorithm + */ + void + setHVAlgorithm (typename boost::shared_ptr > & alg) + { + hv_algorithm_ = alg; + } + + void + setIndices (std::vector & indices) + { + indices_ = indices; + } + + /** + * \brief Sets the input cloud to be classified + */ + void + setInputCloud (const PointInTPtr & cloud) + { + input_ = cloud; + } + + void + setDescriptorName (std::string & name) + { + descr_name_ = name; + } + + void + setTrainingDir (std::string & dir) + { + training_dir_ = dir; + } + + /** + * \brief Performs recognition on the input cloud + */ + + void + recognize (); + + boost::shared_ptr > + getModels () + { + return models_; + } + + boost::shared_ptr > > + getTransforms () + { + return transforms_; + } + + void + setUseCache (bool u) + { + use_cache_ = u; + } + + }; + } +} +#endif /* REC_FRAMEWORK_GLOBAL_PIPELINE_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp new file mode 100644 index 00000000..a9f299df --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_classifier.hpp @@ -0,0 +1,237 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNPipeline::loadFeaturesAndCreateFLANN () + { + boost::shared_ptr < std::vector > models = source_->getModels (); + for (size_t i = 0; i < models->size (); i++) + { + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + bf::path inside = path; + bf::directory_iterator end_itr; + + for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file_name = (itr_in->path ().filename ()).string(); +#else + std::string file_name = (itr_in->path ()).filename (); +#endif + + std::vector < std::string > strs; + boost::split (strs, file_name, boost::is_any_of ("_")); + + if (strs[0] == "descriptor") + { + std::string full_file_name = itr_in->path ().string (); + std::vector < std::string > strs; + boost::split (strs, full_file_name, boost::is_any_of ("/")); + + typename pcl::PointCloud::Ptr signature (new pcl::PointCloud); + pcl::io::loadPCDFile (full_file_name, *signature); + + flann_model descr_model; + descr_model.first = models->at (i); + int size_feat = sizeof(signature->points[0].histogram) / sizeof(float); + descr_model.second.resize (size_feat); + memcpy (&descr_model.second[0], &signature->points[0].histogram[0], size_feat * sizeof(float)); + + flann_models_.push_back (descr_model); + } + } + } + + convertToFLANN (flann_models_, flann_data_); + flann_index_ = new flann::Index (flann_data_, flann::LinearIndexParams ()); + flann_index_->buildIndex (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNPipeline::nearestKSearch (flann::Index * index, const flann_model &model, + int k, flann::Matrix &indices, + flann::Matrix &distances) + { + flann::Matrix p = flann::Matrix (new float[model.second.size ()], 1, model.second.size ()); + memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof(float)); + + indices = flann::Matrix (new int[k], 1, k); + distances = flann::Matrix (new float[k], 1, k); + index->knnSearch (p, indices, distances, k, flann::SearchParams (512)); + delete[] p.ptr (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNPipeline::classify () + { + + categories_.clear (); + confidences_.clear (); + + first_nn_category_ = std::string (""); + + PointInTPtr processed (new pcl::PointCloud); + PointInTPtr in (new pcl::PointCloud); + + typename pcl::PointCloud::CloudVectorType signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + + if (indices_.size ()) + { + pcl::copyPointCloud (*input_, indices_, *in); + } + else + { + in = input_; + } + + estimator_->estimate (in, processed, signatures, centroids); + std::vector indices_scores; + + if (signatures.size () > 0) + { + for (size_t idx = 0; idx < signatures.size (); idx++) + { + float* hist = signatures[idx].points[0].histogram; + int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float); + std::vector std_hist (hist, hist + size_feat); + ModelT empty; + + flann_model histogram (empty, std_hist); + flann::Matrix indices; + flann::Matrix distances; + nearestKSearch (flann_index_, histogram, NN_, indices, distances); + + //gather NN-search results + double score = 0; + for (int i = 0; i < NN_; ++i) + { + score = distances[0][i]; + index_score is; + is.idx_models_ = indices[0][i]; + is.idx_input_ = static_cast (idx); + is.score_ = score; + indices_scores.push_back (is); + } + } + + std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp); + first_nn_category_ = flann_models_[indices_scores[0].idx_models_].first.class_; + + std::map category_map; + std::map::iterator it; + int num_n = std::min (NN_, static_cast (indices_scores.size ())); + + for (int i = 0; i < num_n; ++i) + { + std::string cat = flann_models_[indices_scores[i].idx_models_].first.class_; + it = category_map.find (cat); + if (it == category_map.end ()) + { + category_map[cat] = 1; + } + else + { + it->second++; + } + } + + for (it = category_map.begin (); it != category_map.end (); it++) + { + float prob = static_cast (it->second) / static_cast (num_n); + categories_.push_back (it->first); + confidences_.push_back (prob); + } + + } + else + { + first_nn_category_ = std::string ("error"); + categories_.push_back (first_nn_category_); + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNPipeline::initialize (bool force_retrain) + { + + //use the source to know what has to be trained and what not, checking if the descr_name directory exists + //unless force_retrain is true, then train everything + boost::shared_ptr < std::vector > models = source_->getModels (); + std::cout << "Models size:" << models->size () << std::endl; + + if (force_retrain) + { + for (size_t i = 0; i < models->size (); i++) + { + source_->removeDescDirectory (models->at (i), training_dir_, descr_name_); + } + } + + for (size_t i = 0; i < models->size (); i++) + { + if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_)) + { + for (size_t v = 0; v < models->at (i).views_->size (); v++) + { + PointInTPtr processed (new pcl::PointCloud); + //pro view, compute signatures + typename pcl::PointCloud::CloudVectorType signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + estimator_->estimate (models->at (i).views_->at (v), processed, signatures, centroids); + + //source_->makeModelPersistent (models->at (i), training_dir_, descr_name_, static_cast (v)); + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + + bf::path desc_dir = path; + if (!bf::exists (desc_dir)) + bf::create_directory (desc_dir); + + std::stringstream path_view; + path_view << path << "/view_" << v << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *processed); + + std::stringstream path_pose; + path_pose << path << "/pose_" << v << ".txt"; + PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v)); + + std::stringstream path_entropy; + path_entropy << path << "/entropy_" << v << ".txt"; + PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v)); + + //save signatures and centroids to disk + for (size_t j = 0; j < signatures.size (); j++) + { + std::stringstream path_centroid; + path_centroid << path << "/centroid_" << v << "_" << j << ".txt"; + Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]); + PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid); + + std::stringstream path_descriptor; + path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd"; + pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]); + } + } + + } + else + { + //else skip model + std::cout << "The model has already been trained..." << std::endl; + } + } + + //load features from disk + //initialize FLANN structure + loadFeaturesAndCreateFLANN (); + } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp new file mode 100644 index 00000000..84a9ace8 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_crh.hpp @@ -0,0 +1,498 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include +#include +#include +#include +#include + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix) + { + + if (use_cache_) + { + typedef std::pair mv_pair; + mv_pair pair_model_view = std::make_pair (model.id_, view_id); + + std::map, Eigen::aligned_allocator > >::iterator it = + poses_cache_.find (pair_model_view); + + if (it != poses_cache_.end ()) + { + pose_matrix = it->second; + return; + } + + } + + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/pose_" << view_id << ".txt"; + + PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::getCRH (ModelT & model, int view_id, int d_id, + CRHPointCloud::Ptr & hist) + { + + hist.reset (new CRHPointCloud); + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/crh_" << view_id << "_" << d_id << ".pcd"; + + pcl::io::loadPCDFile (dir.str (), *hist); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::getCentroid (ModelT & model, int view_id, int d_id, + Eigen::Vector3f & centroid) + { + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/centroid_" << view_id << "_" << d_id << ".txt"; + + PersistenceUtils::getCentroidFromFile (dir.str (), centroid); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::getView (ModelT & model, int view_id, PointInTPtr & view) + { + view.reset (new pcl::PointCloud); + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/view_" << view_id << ".pcd"; + pcl::io::loadPCDFile (dir.str (), *view); + + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::loadFeaturesAndCreateFLANN () + { + boost::shared_ptr < std::vector > models = source_->getModels (); + for (size_t i = 0; i < models->size (); i++) + { + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + bf::path inside = path; + bf::directory_iterator end_itr; + + for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file_name = (itr_in->path ().filename ()).string(); +#else + std::string file_name = (itr_in->path ()).filename (); +#endif + + std::vector < std::string > strs; + boost::split (strs, file_name, boost::is_any_of ("_")); + + if (strs[0] == "descriptor") + { + + int view_id = atoi (strs[1].c_str ()); + std::vector < std::string > strs1; + boost::split (strs1, strs[2], boost::is_any_of (".")); + int descriptor_id = atoi (strs1[0].c_str ()); + + std::string full_file_name = itr_in->path ().string (); + typename pcl::PointCloud::Ptr signature (new pcl::PointCloud); + pcl::io::loadPCDFile (full_file_name, *signature); + + flann_model descr_model; + descr_model.model = models->at (i); + descr_model.view_id = view_id; + descr_model.descriptor_id = descriptor_id; + + int size_feat = sizeof(signature->points[0].histogram) / sizeof(float); + descr_model.descr.resize (size_feat); + memcpy (&descr_model.descr[0], &signature->points[0].histogram[0], size_feat * sizeof(float)); + + flann_models_.push_back (descr_model); + + if (use_cache_) + { + + std::stringstream dir_pose; + dir_pose << path << "/pose_" << descr_model.view_id << ".txt"; + + Eigen::Matrix4f pose_matrix; + PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix); + + std::pair pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id); + poses_cache_[pair_model_view] = pose_matrix; + } + } + } + } + + convertToFLANN (flann_models_, flann_data_); + flann_index_ = new flann::Index (flann_data_, flann::LinearIndexParams ()); + flann_index_->buildIndex (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::nearestKSearch (flann::Index * index, const flann_model &model, + int k, flann::Matrix &indices, + flann::Matrix &distances) + { + flann::Matrix p = flann::Matrix (new float[model.descr.size ()], 1, model.descr.size ()); + memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float)); + + indices = flann::Matrix (new int[k], 1, k); + distances = flann::Matrix (new float[k], 1, k); + index->knnSearch (p, indices, distances, k, flann::SearchParams (512)); + delete[] p.ptr (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::recognize () + { + + models_.reset (new std::vector); + transforms_.reset (new std::vector >); + + PointInTPtr processed (new pcl::PointCloud); + PointInTPtr in (new pcl::PointCloud); + + std::vector, Eigen::aligned_allocator > > signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + + if (indices_.size ()) + pcl::copyPointCloud (*input_, indices_, *in); + else + in = input_; + + { + pcl::ScopeTime t ("Estimate feature"); + crh_estimator_->estimate (in, processed, signatures, centroids); + } + + std::vector crh_histograms; + crh_estimator_->getCRHHistograms (crh_histograms); + + std::vector indices_scores; + if (signatures.size () > 0) + { + + { + pcl::ScopeTime t_matching ("Matching and roll..."); + for (size_t idx = 0; idx < signatures.size (); idx++) + { + + float* hist = signatures[idx].points[0].histogram; + int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float); + std::vector std_hist (hist, hist + size_feat); + ModelT empty; + + flann_model histogram; + histogram.descr = std_hist; + + flann::Matrix indices; + flann::Matrix distances; + nearestKSearch (flann_index_, histogram, NN_, indices, distances); + + //gather NN-search results + double score = 0; + for (int i = 0; i < NN_; ++i) + { + score = distances[0][i]; + index_score is; + is.idx_models_ = indices[0][i]; + is.idx_input_ = static_cast (idx); + is.score_ = score; + indices_scores.push_back (is); + } + } + + std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp); + + int num_n = std::min (NN_, static_cast (indices_scores.size ())); + + /* + * Filter some hypothesis regarding to their distance to the first neighbour + */ + + /*std::vector indices_scores_filtered; + indices_scores_filtered.resize (num_n); + indices_scores_filtered[0] = indices_scores[0]; + + float best_score = indices_scores[0].score_; + int kept = 1; + for (int i = 1; i < num_n; ++i) + { + std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl; + if ((best_score / indices_scores[i].score_) > 0.75) + { + indices_scores_filtered[i] = indices_scores[i]; + kept++; + } + + //best_score = indices_scores[i].score_; + } + + indices_scores_filtered.resize (kept); + std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl; + + indices_scores = indices_scores_filtered; + num_n = indices_scores.size ();*/ + + if (do_CRH_) + { + /* + * Once we have the models, we need to find a 6DOF pose using the roll histogram + * pass to pcl_recognition::CRHAlignment both views, centroids and CRH + */ + + pcl::CRHAlignment crha; + + for (int i = 0; i < num_n; ++i) + { + ModelT m = flann_models_[indices_scores[i].idx_models_].model; + int view_id = flann_models_[indices_scores[i].idx_models_].view_id; + int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id; + + std::cout << m.id_ << " " << view_id << " " << desc_id << std::endl; + + //get crhs + CRHPointCloud::Ptr input_crh = crh_histograms[indices_scores[i].idx_input_]; + CRHPointCloud::Ptr view_crh; + getCRH (m, view_id, desc_id, view_crh); + + //get centroids + Eigen::Vector3f input_centroid = centroids[indices_scores[i].idx_input_]; + Eigen::Vector3f view_centroid; + getCentroid (m, view_id, desc_id, view_centroid); + + //crha.setModelAndInputView (view, processed); + crha.setInputAndTargetCentroids (view_centroid, input_centroid); + crha.align (*view_crh, *input_crh); + + Eigen::Matrix4f model_view_pose; + getPose (m, view_id, model_view_pose); + + std::vector < Eigen::Matrix4f, Eigen::aligned_allocator > roll_transforms; + crha.getTransforms (roll_transforms); + + //create object hypothesis + for (size_t k = 0; k < roll_transforms.size (); k++) + { + Eigen::Matrix4f final_roll_trans (roll_transforms[k] * model_view_pose); + models_->push_back (m); + transforms_->push_back (final_roll_trans); + } + } + } + else + { + for (int i = 0; i < num_n; ++i) + { + ModelT m = flann_models_[indices_scores[i].idx_models_].model; + models_->push_back (m); + } + } + } + + std::cout << "Number of object hypotheses:" << models_->size () << std::endl; + + /** + * POSE REFINEMENT + **/ + + if (ICP_iterations_ > 0) + { + pcl::ScopeTime t ("Pose refinement"); + + //Prepare scene and model clouds for the pose refinement step + float VOXEL_SIZE_ICP_ = 0.005f; + PointInTPtr cloud_voxelized_icp (new pcl::PointCloud ()); + pcl::VoxelGrid voxel_grid_icp; + voxel_grid_icp.setInputCloud (processed); + voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_); + voxel_grid_icp.filter (*cloud_voxelized_icp); + source_->voxelizeAllModels (VOXEL_SIZE_ICP_); + +#pragma omp parallel for num_threads(omp_get_num_procs()) + for (int i = 0; i < static_cast (models_->size ()); i++) + { + + ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_); + PointInTPtr model_aligned (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + + pcl::IterativeClosestPoint reg; + reg.setInputSource (model_aligned); //model + reg.setInputTarget (cloud_voxelized_icp); //scene + reg.setMaximumIterations (ICP_iterations_); + reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.f); + reg.setTransformationEpsilon (1e-5); + + typename pcl::PointCloud::Ptr output_ (new pcl::PointCloud ()); + reg.align (*output_); + + Eigen::Matrix4f icp_trans = reg.getFinalTransformation (); + transforms_->at (i) = icp_trans * transforms_->at (i); + } + } + + /** + * HYPOTHESES VERIFICATION + **/ + + if (hv_algorithm_) + { + + pcl::ScopeTime t ("HYPOTHESES VERIFICATION"); + + std::vector::ConstPtr> aligned_models; + aligned_models.resize (models_->size ()); + + for (size_t i = 0; i < models_->size (); i++) + { + ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005f); + PointInTPtr model_aligned (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + aligned_models[i] = model_aligned; + } + + std::vector mask_hv; + hv_algorithm_->setSceneCloud (input_); + hv_algorithm_->addModels (aligned_models, true); + hv_algorithm_->verify (); + hv_algorithm_->getMask (mask_hv); + + boost::shared_ptr < std::vector > models_temp; + boost::shared_ptr < std::vector > > transforms_temp; + + models_temp.reset (new std::vector); + transforms_temp.reset (new std::vector >); + + for (size_t i = 0; i < models_->size (); i++) + { + if (!mask_hv[i]) + continue; + + models_temp->push_back (models_->at (i)); + transforms_temp->push_back (transforms_->at (i)); + } + + models_ = models_temp; + transforms_ = transforms_temp; + } + + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCRHRecognizer::initialize (bool force_retrain) + { + + //use the source to know what has to be trained and what not, checking if the descr_name directory exists + //unless force_retrain is true, then train everything + boost::shared_ptr < std::vector > models = source_->getModels (); + std::cout << "Models size:" << models->size () << std::endl; + + if (force_retrain) + { + for (size_t i = 0; i < models->size (); i++) + { + source_->removeDescDirectory (models->at (i), training_dir_, descr_name_); + } + } + + for (size_t i = 0; i < models->size (); i++) + { + if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_)) + { + for (size_t v = 0; v < models->at (i).views_->size (); v++) + { + PointInTPtr processed (new pcl::PointCloud); + PointInTPtr view = models->at (i).views_->at (v); + + if (noisify_) + { + double noise_std = noise_; + boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration duration( time.time_of_day() ); + boost::mt19937 rng; + rng.seed (static_cast (duration.total_milliseconds())); + boost::normal_distribution<> nd (0.0, noise_std); + boost::variate_generator > var_nor (rng, nd); + // Noisify each point in the dataset + for (size_t cp = 0; cp < view->points.size (); ++cp) + view->points[cp].z += static_cast (var_nor ()); + + } + + //pro view, compute signatures and CRH + std::vector, Eigen::aligned_allocator > > signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + crh_estimator_->estimate (view, processed, signatures, centroids); + + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + + bf::path desc_dir = path; + if (!bf::exists (desc_dir)) + bf::create_directory (desc_dir); + + std::stringstream path_view; + path_view << path << "/view_" << v << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *processed); + + std::stringstream path_pose; + path_pose << path << "/pose_" << v << ".txt"; + PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v)); + + std::stringstream path_entropy; + path_entropy << path << "/entropy_" << v << ".txt"; + PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v)); + + std::vector crh_histograms; + crh_estimator_->getCRHHistograms (crh_histograms); + + //save signatures and centroids to disk + for (size_t j = 0; j < signatures.size (); j++) + { + std::stringstream path_centroid; + path_centroid << path << "/centroid_" << v << "_" << j << ".txt"; + Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]); + PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid); + + std::stringstream path_descriptor; + path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd"; + pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]); + + std::stringstream path_roll; + path_roll << path << "/crh_" << v << "_" << j << ".pcd"; + pcl::io::savePCDFileBinary (path_roll.str (), *crh_histograms[j]); + } + } + + } + else + { + //else skip model + std::cout << "The model has already been trained..." << std::endl; + } + } + + //load features from disk + //initialize FLANN structure + loadFeaturesAndCreateFLANN (); + } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp new file mode 100644 index 00000000..c209976c --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/global_nn_recognizer_cvfh.hpp @@ -0,0 +1,734 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include +#include +#include +#include +#include + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix) + { + + if (use_cache_) + { + typedef std::pair mv_pair; + mv_pair pair_model_view = std::make_pair (model.id_, view_id); + + std::map, Eigen::aligned_allocator > >::iterator it = + poses_cache_.find (pair_model_view); + + if (it != poses_cache_.end ()) + { + pose_matrix = it->second; + return; + } + + } + + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/pose_" << view_id << ".txt"; + + PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix); + } + +template class Distance, typename PointInT, typename FeatureT> + bool + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::getRollPose (ModelT & model, int view_id, int d_id, + Eigen::Matrix4f & pose_matrix) + { + + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + + dir << path << "/roll_trans_" << view_id << "_" << d_id << ".txt"; + + bf::path file_path = dir.str (); + if (bf::exists (file_path)) + { + PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix); + return true; + } + else + { + return false; + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::getCentroid (ModelT & model, int view_id, int d_id, + Eigen::Vector3f & centroid) + { + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/centroid_" << view_id << "_" << d_id << ".txt"; + + PersistenceUtils::getCentroidFromFile (dir.str (), centroid); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::getView (ModelT & model, int view_id, PointInTPtr & view) + { + view.reset (new pcl::PointCloud); + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/view_" << view_id << ".pcd"; + pcl::io::loadPCDFile (dir.str (), *view); + + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::loadFeaturesAndCreateFLANN () + { + + boost::shared_ptr < std::vector > models = source_->getModels (); + + std::map < std::string, boost::shared_ptr > > single_categories; + if (use_single_categories_) + { + for (size_t i = 0; i < models->size (); i++) + { + std::map > >::iterator it; + std::string cat_model = models->at (i).class_; + it = single_categories.find (cat_model); + if (it == single_categories.end ()) + { + boost::shared_ptr < std::vector > v (new std::vector); + single_categories[cat_model] = v; + } + } + } + + for (size_t i = 0; i < models->size (); i++) + { + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + bf::path inside = path; + bf::directory_iterator end_itr; + + for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file_name = (itr_in->path ().filename ()).string(); +#else + std::string file_name = (itr_in->path ()).filename (); +#endif + + std::vector < std::string > strs; + boost::split (strs, file_name, boost::is_any_of ("_")); + + if (strs[0] == "descriptor") + { + + int view_id = atoi (strs[1].c_str ()); + std::vector < std::string > strs1; + boost::split (strs1, strs[2], boost::is_any_of (".")); + int descriptor_id = atoi (strs1[0].c_str ()); + + std::string full_file_name = itr_in->path ().string (); + typename pcl::PointCloud::Ptr signature (new pcl::PointCloud); + pcl::io::loadPCDFile (full_file_name, *signature); + + flann_model descr_model; + descr_model.model = models->at (i); + descr_model.view_id = view_id; + descr_model.descriptor_id = descriptor_id; + + int size_feat = sizeof(signature->points[0].histogram) / sizeof(float); + descr_model.descr.resize (size_feat); + memcpy (&descr_model.descr[0], &signature->points[0].histogram[0], size_feat * sizeof(float)); + + if (use_single_categories_) + { + std::map > >::iterator it; + std::string cat_model = models->at (i).class_; + it = single_categories.find (cat_model); + if (it == single_categories.end ()) + { + std::cout << cat_model << std::endl; + std::cout << "Should not happen..." << std::endl; + } + else + { + it->second->push_back (static_cast (flann_models_.size ())); + } + } + + flann_models_.push_back (descr_model); + + if (use_cache_) + { + + std::stringstream dir_pose; + dir_pose << path << "/pose_" << descr_model.view_id << ".txt"; + + Eigen::Matrix4f pose_matrix; + PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix); + + std::pair pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id); + poses_cache_[pair_model_view] = pose_matrix; + } + } + } + } + + convertToFLANN (flann_models_, flann_data_); + flann_index_ = new flann::Index (flann_data_, flann::LinearIndexParams ()); + flann_index_->buildIndex (); + + //single categories... + if (use_single_categories_) + { + std::map > >::iterator it; + + single_categories_data_.resize (single_categories.size ()); + single_categories_index_.resize (single_categories.size ()); + single_categories_pointers_to_models_.resize (single_categories.size ()); + + int kk = 0; + for (it = single_categories.begin (); it != single_categories.end (); it++) + { + //create index and flann data + convertToFLANN (flann_models_, it->second, single_categories_data_[kk]); + single_categories_index_[kk] = new flann::Index (single_categories_data_[kk], flann::LinearIndexParams ()); + single_categories_pointers_to_models_[kk] = it->second; + + category_to_vectors_indices_[it->first] = kk; + kk++; + } + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::nearestKSearch (flann::Index * index, const flann_model &model, + int k, flann::Matrix &indices, + flann::Matrix &distances) + { + flann::Matrix p = flann::Matrix (new float[model.descr.size ()], 1, model.descr.size ()); + memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float)); + + indices = flann::Matrix (new int[k], 1, k); + distances = flann::Matrix (new float[k], 1, k); + index->knnSearch (p, indices, distances, k, flann::SearchParams (512)); + delete[] p.ptr (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::recognize () + { + + models_.reset (new std::vector); + transforms_.reset (new std::vector >); + + PointInTPtr processed (new pcl::PointCloud); + PointInTPtr in (new pcl::PointCloud); + + std::vector, Eigen::aligned_allocator > > signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + + if (indices_.size ()) + pcl::copyPointCloud (*input_, indices_, *in); + else + in = input_; + + { + pcl::ScopeTime t ("Estimate feature"); + micvfh_estimator_->estimate (in, processed, signatures, centroids); + } + + std::vector indices_scores; + descriptor_distances_.clear (); + + if (signatures.size () > 0) + { + + { + pcl::ScopeTime t_matching ("Matching and roll..."); + + if (use_single_categories_ && (categories_to_be_searched_.size () > 0)) + { + + //perform search of the different signatures in the categories_to_be_searched_ + for (size_t c = 0; c < categories_to_be_searched_.size (); c++) + { + std::cout << "Using category:" << categories_to_be_searched_[c] << std::endl; + for (size_t idx = 0; idx < signatures.size (); idx++) + { + /*float* hist = signatures[idx].points[0].histogram; + std::vector std_hist (hist, hist + getHistogramLength (dummy)); + flann_model histogram ("", std_hist); + flann::Matrix indices; + flann::Matrix distances; + + std::map::iterator it; + it = category_to_vectors_indices_.find (categories_to_be_searched_[c]); + + assert (it != category_to_vectors_indices_.end ()); + nearestKSearch (single_categories_index_[it->second], histogram, nmodels_, indices, distances);*/ + + float* hist = signatures[idx].points[0].histogram; + int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float); + std::vector std_hist (hist, hist + size_feat); + //ModelT empty; + + flann_model histogram; + histogram.descr = std_hist; + flann::Matrix indices; + flann::Matrix distances; + + std::map::iterator it; + it = category_to_vectors_indices_.find (categories_to_be_searched_[c]); + assert (it != category_to_vectors_indices_.end ()); + + nearestKSearch (single_categories_index_[it->second], histogram, NN_, indices, distances); + //gather NN-search results + double score = 0; + for (size_t i = 0; i < NN_; ++i) + { + score = distances[0][i]; + index_score is; + is.idx_models_ = single_categories_pointers_to_models_[it->second]->at (indices[0][i]); + is.idx_input_ = static_cast (idx); + is.score_ = score; + indices_scores.push_back (is); + } + } + + //we cannot add more than nmodels per category, so sort here and remove offending ones... + std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp); + indices_scores.resize ((c + 1) * NN_); + } + } + else + { + for (size_t idx = 0; idx < signatures.size (); idx++) + { + + float* hist = signatures[idx].points[0].histogram; + int size_feat = sizeof(signatures[idx].points[0].histogram) / sizeof(float); + std::vector std_hist (hist, hist + size_feat); + //ModelT empty; + + flann_model histogram; + histogram.descr = std_hist; + + flann::Matrix indices; + flann::Matrix distances; + nearestKSearch (flann_index_, histogram, NN_, indices, distances); + + //gather NN-search results + double score = 0; + for (int i = 0; i < NN_; ++i) + { + score = distances[0][i]; + index_score is; + is.idx_models_ = indices[0][i]; + is.idx_input_ = static_cast (idx); + is.score_ = score; + indices_scores.push_back (is); + + //ModelT m = flann_models_[indices[0][i]].model; + } + } + } + + std::sort (indices_scores.begin (), indices_scores.end (), sortIndexScoresOp); + + /* + * There might be duplicated candidates, in those cases it makes sense to take + * the closer one in descriptor space + */ + + typename std::map found; + typename std::map::iterator it_map; + for (size_t i = 0; i < indices_scores.size (); i++) + { + flann_model m = flann_models_[indices_scores[i].idx_models_]; + it_map = found.find (m); + if (it_map == found.end ()) + { + indices_scores[found.size ()] = indices_scores[i]; + found[m] = true; + } + } + indices_scores.resize (found.size ()); + + int num_n = std::min (NN_, static_cast (indices_scores.size ())); + + /* + * Filter some hypothesis regarding to their distance to the first neighbour + */ + + /*std::vector indices_scores_filtered; + indices_scores_filtered.resize (num_n); + indices_scores_filtered[0] = indices_scores[0]; + + float best_score = indices_scores[0].score_; + int kept = 1; + for (int i = 1; i < num_n; ++i) + { + //std::cout << best_score << indices_scores[i].score_ << (best_score / indices_scores[i].score_) << std::endl; + if ((best_score / indices_scores[i].score_) > 0.65) + { + indices_scores_filtered[i] = indices_scores[i]; + kept++; + } + + //best_score = indices_scores[i].score_; + } + + indices_scores_filtered.resize (kept); + //std::cout << indices_scores_filtered.size () << " § " << num_n << std::endl; + + indices_scores = indices_scores_filtered; + num_n = indices_scores.size ();*/ + std::cout << "Number of object hypotheses... " << num_n << std::endl; + + std::vector valid_trans; + std::vector < Eigen::Matrix4f, Eigen::aligned_allocator > transformations; + + micvfh_estimator_->getValidTransformsVec (valid_trans); + micvfh_estimator_->getTransformsVec (transformations); + + for (int i = 0; i < num_n; ++i) + { + ModelT m = flann_models_[indices_scores[i].idx_models_].model; + int view_id = flann_models_[indices_scores[i].idx_models_].view_id; + int desc_id = flann_models_[indices_scores[i].idx_models_].descriptor_id; + + int idx_input = indices_scores[i].idx_input_; + + std::cout << m.class_ << "/" << m.id_ << " ==> " << indices_scores[i].score_ << std::endl; + + Eigen::Matrix4f roll_view_pose; + bool roll_pose_found = getRollPose (m, view_id, desc_id, roll_view_pose); + + if (roll_pose_found && valid_trans[idx_input]) + { + Eigen::Matrix4f transposed = roll_view_pose.transpose (); + + //std::cout << transposed << std::endl; + + PointInTPtr view; + getView (m, view_id, view); + + Eigen::Matrix4f model_view_pose; + getPose (m, view_id, model_view_pose); + + Eigen::Matrix4f scale_mat; + scale_mat.setIdentity (4, 4); + + if (compute_scale_) + { + //compute scale using the whole view + PointInTPtr view_transformed (new pcl::PointCloud); + Eigen::Matrix4f hom_from_OVC_to_CC; + hom_from_OVC_to_CC = transformations[idx_input].inverse () * transposed; + pcl::transformPointCloud (*view, *view_transformed, hom_from_OVC_to_CC); + + Eigen::Vector3f input_centroid = centroids[indices_scores[i].idx_input_]; + Eigen::Vector3f view_centroid; + getCentroid (m, view_id, desc_id, view_centroid); + + Eigen::Vector4f cmatch4f (view_centroid[0], view_centroid[1], view_centroid[2], 0); + Eigen::Vector4f cinput4f (input_centroid[0], input_centroid[1], input_centroid[2], 0); + + Eigen::Vector4f max_pt_input; + pcl::getMaxDistance (*processed, cinput4f, max_pt_input); + max_pt_input[3] = 0; + float max_dist_input = (cinput4f - max_pt_input).norm (); + + //compute max dist for transformed model_view + pcl::getMaxDistance (*view, cmatch4f, max_pt_input); + max_pt_input[3] = 0; + float max_dist_view = (cmatch4f - max_pt_input).norm (); + + cmatch4f = hom_from_OVC_to_CC * cmatch4f; + std::cout << max_dist_view << " " << max_dist_input << std::endl; + + float scale_factor_view = max_dist_input / max_dist_view; + std::cout << "Scale factor:" << scale_factor_view << std::endl; + + Eigen::Matrix4f center, center_inv; + + center.setIdentity (4, 4); + center (0, 3) = -cinput4f[0]; + center (1, 3) = -cinput4f[1]; + center (2, 3) = -cinput4f[2]; + + center_inv.setIdentity (4, 4); + center_inv (0, 3) = cinput4f[0]; + center_inv (1, 3) = cinput4f[1]; + center_inv (2, 3) = cinput4f[2]; + + scale_mat (0, 0) = scale_factor_view; + scale_mat (1, 1) = scale_factor_view; + scale_mat (2, 2) = scale_factor_view; + + scale_mat = center_inv * scale_mat * center; + } + + Eigen::Matrix4f hom_from_OC_to_CC; + hom_from_OC_to_CC = scale_mat * transformations[idx_input].inverse () * transposed * model_view_pose; + + models_->push_back (m); + transforms_->push_back (hom_from_OC_to_CC); + descriptor_distances_.push_back (static_cast (indices_scores[i].score_)); + } + else + { + PCL_WARN("The roll pose was not found, should use CRH here... \n"); + } + } + } + + std::cout << "Number of object hypotheses:" << models_->size () << std::endl; + + /** + * POSE REFINEMENT + **/ + + if (ICP_iterations_ > 0) + { + pcl::ScopeTime t ("Pose refinement"); + + //Prepare scene and model clouds for the pose refinement step + float VOXEL_SIZE_ICP_ = 0.005f; + PointInTPtr cloud_voxelized_icp (new pcl::PointCloud ()); + + { + pcl::ScopeTime t ("Voxelize stuff..."); + pcl::VoxelGrid voxel_grid_icp; + voxel_grid_icp.setInputCloud (processed); + voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_); + voxel_grid_icp.filter (*cloud_voxelized_icp); + source_->voxelizeAllModels (VOXEL_SIZE_ICP_); + } + +#pragma omp parallel for num_threads(omp_get_num_procs()) + for (int i = 0; i < static_cast (models_->size ()); i++) + { + + ConstPointInTPtr model_cloud; + PointInTPtr model_aligned (new pcl::PointCloud); + + if (compute_scale_) + { + model_cloud = models_->at (i).getAssembled (-1); + PointInTPtr model_aligned_m (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i)); + pcl::VoxelGrid voxel_grid_icp; + voxel_grid_icp.setInputCloud (model_aligned_m); + voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_); + voxel_grid_icp.filter (*model_aligned); + } + else + { + model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + } + + pcl::IterativeClosestPoint reg; + reg.setInputSource (model_aligned); //model + reg.setInputTarget (cloud_voxelized_icp); //scene + reg.setMaximumIterations (ICP_iterations_); + reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 3.f); + reg.setTransformationEpsilon (1e-6); + + typename pcl::PointCloud::Ptr output_ (new pcl::PointCloud ()); + reg.align (*output_); + + Eigen::Matrix4f icp_trans = reg.getFinalTransformation (); + transforms_->at (i) = icp_trans * transforms_->at (i); + } + } + + /** + * HYPOTHESES VERIFICATION + **/ + + if (hv_algorithm_) + { + + pcl::ScopeTime t ("HYPOTHESES VERIFICATION"); + + std::vector::ConstPtr> aligned_models; + aligned_models.resize (models_->size ()); + + for (size_t i = 0; i < models_->size (); i++) + { + ConstPointInTPtr model_cloud; + PointInTPtr model_aligned (new pcl::PointCloud); + + if (compute_scale_) + { + model_cloud = models_->at (i).getAssembled (-1); + PointInTPtr model_aligned_m (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned_m, transforms_->at (i)); + pcl::VoxelGrid voxel_grid_icp; + voxel_grid_icp.setInputCloud (model_aligned_m); + voxel_grid_icp.setLeafSize (0.005f, 0.005f, 0.005f); + voxel_grid_icp.filter (*model_aligned); + } + else + { + model_cloud = models_->at (i).getAssembled (0.005f); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + } + + //ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.005f); + //PointInTPtr model_aligned (new pcl::PointCloud); + //pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + aligned_models[i] = model_aligned; + } + + std::vector mask_hv; + hv_algorithm_->setSceneCloud (input_); + hv_algorithm_->addModels (aligned_models, true); + hv_algorithm_->verify (); + hv_algorithm_->getMask (mask_hv); + + boost::shared_ptr < std::vector > models_temp; + boost::shared_ptr < std::vector > > transforms_temp; + + models_temp.reset (new std::vector); + transforms_temp.reset (new std::vector >); + + for (size_t i = 0; i < models_->size (); i++) + { + if (!mask_hv[i]) + continue; + + models_temp->push_back (models_->at (i)); + transforms_temp->push_back (transforms_->at (i)); + } + + models_ = models_temp; + transforms_ = transforms_temp; + } + + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::GlobalNNCVFHRecognizer::initialize (bool force_retrain) + { + + //use the source to know what has to be trained and what not, checking if the descr_name directory exists + //unless force_retrain is true, then train everything + boost::shared_ptr < std::vector > models = source_->getModels (); + std::cout << "Models size:" << models->size () << std::endl; + + if (force_retrain) + { + for (size_t i = 0; i < models->size (); i++) + { + source_->removeDescDirectory (models->at (i), training_dir_, descr_name_); + } + } + + for (size_t i = 0; i < models->size (); i++) + { + if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_)) + { + for (size_t v = 0; v < models->at (i).views_->size (); v++) + { + PointInTPtr processed (new pcl::PointCloud); + PointInTPtr view = models->at (i).views_->at (v); + + if (view->points.size () == 0) + PCL_WARN("View has no points!!!\n"); + + if (noisify_) + { + double noise_std = noise_; + boost::posix_time::ptime time = boost::posix_time::microsec_clock::local_time(); + boost::posix_time::time_duration duration( time.time_of_day() ); + boost::mt19937 rng; + rng.seed (static_cast (duration.total_milliseconds())); + boost::normal_distribution<> nd (0.0, noise_std); + boost::variate_generator > var_nor (rng, nd); + // Noisify each point in the dataset + for (size_t cp = 0; cp < view->points.size (); ++cp) + view->points[cp].z += static_cast (var_nor ()); + + } + + //pro view, compute signatures + std::vector, Eigen::aligned_allocator > > signatures; + std::vector < Eigen::Vector3f, Eigen::aligned_allocator > centroids; + micvfh_estimator_->estimate (view, processed, signatures, centroids); + + std::vector valid_trans; + std::vector < Eigen::Matrix4f, Eigen::aligned_allocator > transforms; + + micvfh_estimator_->getValidTransformsVec (valid_trans); + micvfh_estimator_->getTransformsVec (transforms); + + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + + bf::path desc_dir = path; + if (!bf::exists (desc_dir)) + bf::create_directory (desc_dir); + + std::stringstream path_view; + path_view << path << "/view_" << v << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *processed); + + std::stringstream path_pose; + path_pose << path << "/pose_" << v << ".txt"; + PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v)); + + std::stringstream path_entropy; + path_entropy << path << "/entropy_" << v << ".txt"; + PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v)); + + //save signatures and centroids to disk + for (size_t j = 0; j < signatures.size (); j++) + { + if (valid_trans[j]) + { + std::stringstream path_centroid; + path_centroid << path << "/centroid_" << v << "_" << j << ".txt"; + Eigen::Vector3f centroid (centroids[j][0], centroids[j][1], centroids[j][2]); + PersistenceUtils::writeCentroidToFile (path_centroid.str (), centroid); + + std::stringstream path_descriptor; + path_descriptor << path << "/descriptor_" << v << "_" << j << ".pcd"; + pcl::io::savePCDFileBinary (path_descriptor.str (), signatures[j]); + + //save roll transform + std::stringstream path_pose; + path_pose << path << "/roll_trans_" << v << "_" << j << ".txt"; + PersistenceUtils::writeMatrixToFile (path_pose.str (), transforms[j]); + } + } + } + + } + else + { + //else skip model + std::cout << "The model has already been trained..." << std::endl; + //there is no need to keep the views in memory once the model has been trained + models->at (i).views_->clear (); + } + } + + //load features from disk + //initialize FLANN structure + loadFeaturesAndCreateFLANN (); + } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp new file mode 100644 index 00000000..77854bf4 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/impl/local_recognizer.hpp @@ -0,0 +1,510 @@ +#include +#include +#include +#include +#include +#include +#include + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::loadFeaturesAndCreateFLANN () + { + boost::shared_ptr < std::vector > models = source_->getModels (); + std::cout << "Models size:" << models->size () << std::endl; + + for (size_t i = 0; i < models->size (); i++) + { + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + bf::path inside = path; + bf::directory_iterator end_itr; + + for (bf::directory_iterator itr_in (inside); itr_in != end_itr; ++itr_in) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file_name = (itr_in->path ().filename ()).string(); +#else + std::string file_name = (itr_in->path ()).filename (); +#endif + + std::vector < std::string > strs; + boost::split (strs, file_name, boost::is_any_of ("_")); + + if (strs[0] == "descriptor") + { + std::string full_file_name = itr_in->path ().string (); + std::string name = file_name.substr (0, file_name.length () - 4); + std::vector < std::string > strs; + boost::split (strs, name, boost::is_any_of ("_")); + + flann_model descr_model; + descr_model.model = models->at (i); + descr_model.view_id = atoi (strs[1].c_str ()); + + if (use_cache_) + { + + std::stringstream dir_keypoints; + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + dir_keypoints << path << "/keypoint_indices_" << descr_model.view_id << ".pcd"; + + std::stringstream dir_pose; + dir_pose << path << "/pose_" << descr_model.view_id << ".txt"; + + Eigen::Matrix4f pose_matrix; + PersistenceUtils::readMatrixFromFile (dir_pose.str (), pose_matrix); + + std::pair pair_model_view = std::make_pair (models->at (i).id_, descr_model.view_id); + poses_cache_[pair_model_view] = pose_matrix; + + //load keypoints and save them to cache + typename pcl::PointCloud::Ptr keypoints (new pcl::PointCloud ()); + pcl::io::loadPCDFile (dir_keypoints.str (), *keypoints); + keypoints_cache_[pair_model_view] = keypoints; + } + + typename pcl::PointCloud::Ptr signature (new pcl::PointCloud ()); + pcl::io::loadPCDFile (full_file_name, *signature); + + int size_feat = sizeof(signature->points[0].histogram) / sizeof(float); + + for (size_t dd = 0; dd < signature->points.size (); dd++) + { + descr_model.keypoint_id = static_cast (dd); + descr_model.descr.resize (size_feat); + + memcpy (&descr_model.descr[0], &signature->points[dd].histogram[0], size_feat * sizeof(float)); + + flann_models_.push_back (descr_model); + } + } + } + } + + convertToFLANN (flann_models_, flann_data_); + + flann_index_ = new flann::Index (flann_data_, flann::KDTreeIndexParams (4)); + flann_index_->buildIndex (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::nearestKSearch (flann::Index * index, + const flann_model &model, int k, + flann::Matrix &indices, + flann::Matrix &distances) + { + flann::Matrix p = flann::Matrix (new float[model.descr.size ()], 1, model.descr.size ()); + memcpy (&p.ptr ()[0], &model.descr[0], p.cols * p.rows * sizeof(float)); + + indices = flann::Matrix (new int[k], 1, k); + distances = flann::Matrix (new float[k], 1, k); + index->knnSearch (p, indices, distances, k, flann::SearchParams (kdtree_splits_)); + delete[] p.ptr (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::initialize (bool force_retrain) + { + boost::shared_ptr < std::vector > models; + + if(search_model_.compare("") == 0) { + models = source_->getModels (); + } else { + models = source_->getModels (search_model_); + //reset cache and flann structures + if(flann_index_ != 0) + delete flann_index_; + + flann_models_.clear(); + poses_cache_.clear(); + keypoints_cache_.clear(); + } + + std::cout << "Models size:" << models->size () << std::endl; + + if (force_retrain) + { + for (size_t i = 0; i < models->size (); i++) + { + source_->removeDescDirectory (models->at (i), training_dir_, descr_name_); + } + } + + for (size_t i = 0; i < models->size (); i++) + { + std::cout << models->at (i).class_ << " " << models->at (i).id_ << std::endl; + + if (!source_->modelAlreadyTrained (models->at (i), training_dir_, descr_name_)) + { + for (size_t v = 0; v < models->at (i).views_->size (); v++) + { + PointInTPtr processed (new pcl::PointCloud); + typename pcl::PointCloud::Ptr signatures (new pcl::PointCloud ()); + PointInTPtr keypoints_pointcloud; + + bool success = estimator_->estimate (models->at (i).views_->at (v), processed, keypoints_pointcloud, signatures); + + if (success) + { + std::string path = source_->getModelDescriptorDir (models->at (i), training_dir_, descr_name_); + + bf::path desc_dir = path; + if (!bf::exists (desc_dir)) + bf::create_directory (desc_dir); + + std::stringstream path_view; + path_view << path << "/view_" << v << ".pcd"; + pcl::io::savePCDFileBinary (path_view.str (), *processed); + + std::stringstream path_pose; + path_pose << path << "/pose_" << v << ".txt"; + PersistenceUtils::writeMatrixToFile (path_pose.str (), models->at (i).poses_->at (v)); + + if(v < models->at (i).self_occlusions_->size()) { + std::stringstream path_entropy; + path_entropy << path << "/entropy_" << v << ".txt"; + PersistenceUtils::writeFloatToFile (path_entropy.str (), models->at (i).self_occlusions_->at (v)); + } + + //save keypoints and signatures to disk + std::stringstream keypoints_sstr; + keypoints_sstr << path << "/keypoint_indices_" << v << ".pcd"; + + /*boost::shared_ptr < std::vector > indices (new std::vector ()); + indices->resize (keypoints.points.size ()); + for (size_t kk = 0; kk < indices->size (); kk++) + (*indices)[kk] = keypoints.points[kk]; + typename pcl::PointCloud keypoints_pointcloud; + pcl::copyPointCloud (*processed, *indices, keypoints_pointcloud);*/ + pcl::io::savePCDFileBinary (keypoints_sstr.str (), *keypoints_pointcloud); + + std::stringstream path_descriptor; + path_descriptor << path << "/descriptor_" << v << ".pcd"; + pcl::io::savePCDFileBinary (path_descriptor.str (), *signatures); + } + } + } else { + std::cout << "Model already trained..." << std::endl; + //there is no need to keep the views in memory once the model has been trained + models->at (i).views_->clear(); + } + } + + loadFeaturesAndCreateFLANN (); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::recognize () + { + + models_.reset (new std::vector); + transforms_.reset (new std::vector >); + + PointInTPtr processed; + typename pcl::PointCloud::Ptr signatures (new pcl::PointCloud ()); + //pcl::PointCloud keypoints_input; + PointInTPtr keypoints_pointcloud; + + if (signatures_ != 0 && processed_ != 0 && (signatures_->size () == keypoints_pointcloud->points.size ())) + { + keypoints_pointcloud = keypoints_input_; + signatures = signatures_; + processed = processed_; + std::cout << "Using the ISPK ..." << std::endl; + } + else + { + processed.reset( (new pcl::PointCloud)); + if (indices_.size () > 0) + { + PointInTPtr sub_input (new pcl::PointCloud); + pcl::copyPointCloud (*input_, indices_, *sub_input); + estimator_->estimate (sub_input, processed, keypoints_pointcloud, signatures); + } + else + { + estimator_->estimate (input_, processed, keypoints_pointcloud, signatures); + } + + processed_ = processed; + + } + + std::cout << "Number of keypoints:" << keypoints_pointcloud->points.size () << std::endl; + + int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float); + + //feature matching and object hypotheses + std::map object_hypotheses; + { + for (size_t idx = 0; idx < signatures->points.size (); idx++) + { + float* hist = signatures->points[idx].histogram; + std::vector std_hist (hist, hist + size_feat); + flann_model histogram; + histogram.descr = std_hist; + flann::Matrix indices; + flann::Matrix distances; + nearestKSearch (flann_index_, histogram, 1, indices, distances); + + //read view pose and keypoint coordinates, transform keypoint coordinates to model coordinates + Eigen::Matrix4f homMatrixPose; + getPose (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, homMatrixPose); + + typename pcl::PointCloud::Ptr keypoints (new pcl::PointCloud ()); + getKeypoints (flann_models_.at (indices[0][0]).model, flann_models_.at (indices[0][0]).view_id, keypoints); + + PointInT view_keypoint = keypoints->points[flann_models_.at (indices[0][0]).keypoint_id]; + PointInT model_keypoint; + model_keypoint.getVector4fMap () = homMatrixPose.inverse () * view_keypoint.getVector4fMap (); + + typename std::map::iterator it_map; + if ((it_map = object_hypotheses.find (flann_models_.at (indices[0][0]).model.id_)) != object_hypotheses.end ()) + { + //if the object hypothesis already exists, then add information + ObjectHypothesis oh = (*it_map).second; + oh.correspondences_pointcloud->points.push_back (model_keypoint); + oh.correspondences_to_inputcloud->push_back ( + pcl::Correspondence (static_cast (oh.correspondences_pointcloud->points.size () - 1), + static_cast (idx), distances[0][0])); + oh.feature_distances_->push_back (distances[0][0]); + + } + else + { + //create object hypothesis + ObjectHypothesis oh; + + typename pcl::PointCloud::Ptr correspondences_pointcloud (new pcl::PointCloud ()); + correspondences_pointcloud->points.push_back (model_keypoint); + + oh.model_ = flann_models_.at (indices[0][0]).model; + oh.correspondences_pointcloud = correspondences_pointcloud; + //last keypoint for this model is a correspondence the current scene keypoint + + pcl::CorrespondencesPtr corr (new pcl::Correspondences ()); + oh.correspondences_to_inputcloud = corr; + oh.correspondences_to_inputcloud->push_back (pcl::Correspondence (0, static_cast (idx), distances[0][0])); + + boost::shared_ptr < std::vector > feat_dist (new std::vector); + feat_dist->push_back (distances[0][0]); + + oh.feature_distances_ = feat_dist; + object_hypotheses[oh.model_.id_] = oh; + } + } + } + + typename std::map::iterator it_map; + + std::vector feature_distance_avg; + + { + //pcl::ScopeTime t("Geometric verification, RANSAC and transform estimation"); + for (it_map = object_hypotheses.begin (); it_map != object_hypotheses.end (); it_map++) + { + std::vector < pcl::Correspondences > corresp_clusters; + cg_algorithm_->setSceneCloud (keypoints_pointcloud); + cg_algorithm_->setInputCloud ((*it_map).second.correspondences_pointcloud); + cg_algorithm_->setModelSceneCorrespondences ((*it_map).second.correspondences_to_inputcloud); + cg_algorithm_->cluster (corresp_clusters); + + std::cout << "Instances:" << corresp_clusters.size () << " Total correspondences:" << (*it_map).second.correspondences_to_inputcloud->size () << " " << it_map->first << std::endl; + std::vector good_indices_for_hypothesis (corresp_clusters.size (), true); + + if (threshold_accept_model_hypothesis_ < 1.f) + { + //sort the hypotheses for each model according to their correspondences and take those that are threshold_accept_model_hypothesis_ over the max cardinality + int max_cardinality = -1; + for (size_t i = 0; i < corresp_clusters.size (); i++) + { + //std::cout << (corresp_clusters[i]).size() << " -- " << (*(*it_map).second.correspondences_to_inputcloud).size() << std::endl; + if (max_cardinality < static_cast (corresp_clusters[i].size ())) + { + max_cardinality = static_cast (corresp_clusters[i].size ()); + } + } + + for (size_t i = 0; i < corresp_clusters.size (); i++) + { + if (static_cast ((corresp_clusters[i]).size ()) < (threshold_accept_model_hypothesis_ * static_cast (max_cardinality))) + { + good_indices_for_hypothesis[i] = false; + } + } + } + + for (size_t i = 0; i < corresp_clusters.size (); i++) + { + + if (!good_indices_for_hypothesis[i]) + continue; + + //drawCorrespondences (processed, it_map->second, keypoints_pointcloud, corresp_clusters[i]); + + Eigen::Matrix4f best_trans; + typename pcl::registration::TransformationEstimationSVD < PointInT, PointInT > t_est; + t_est.estimateRigidTransformation (*(*it_map).second.correspondences_pointcloud, *keypoints_pointcloud, corresp_clusters[i], best_trans); + + models_->push_back ((*it_map).second.model_); + transforms_->push_back (best_trans); + + } + } + } + + std::cout << "Number of hypotheses:" << models_->size() << std::endl; + + /** + * POSE REFINEMENT + **/ + + if (ICP_iterations_ > 0) + { + pcl::ScopeTime ticp ("ICP "); + + //Prepare scene and model clouds for the pose refinement step + PointInTPtr cloud_voxelized_icp (new pcl::PointCloud ()); + pcl::VoxelGrid voxel_grid_icp; + voxel_grid_icp.setInputCloud (processed); + voxel_grid_icp.setLeafSize (VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_, VOXEL_SIZE_ICP_); + voxel_grid_icp.filter (*cloud_voxelized_icp); + source_->voxelizeAllModels (VOXEL_SIZE_ICP_); + +#pragma omp parallel for schedule(dynamic,1) num_threads(omp_get_num_procs()) + for (int i = 0; i < static_cast(models_->size ()); i++) + { + + ConstPointInTPtr model_cloud; + PointInTPtr model_aligned (new pcl::PointCloud); + model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + + typename pcl::registration::CorrespondenceRejectorSampleConsensus::Ptr rej ( + new pcl::registration::CorrespondenceRejectorSampleConsensus ()); + + rej->setInputTarget (cloud_voxelized_icp); + rej->setMaximumIterations (1000); + rej->setInlierThreshold (0.005f); + rej->setInputSource (model_aligned); + + pcl::IterativeClosestPoint reg; + reg.addCorrespondenceRejector (rej); + reg.setInputTarget (cloud_voxelized_icp); //scene + reg.setInputSource (model_aligned); //model + reg.setMaximumIterations (ICP_iterations_); + reg.setMaxCorrespondenceDistance (VOXEL_SIZE_ICP_ * 4.f); + + typename pcl::PointCloud::Ptr output_ (new pcl::PointCloud ()); + reg.align (*output_); + + Eigen::Matrix4f icp_trans = reg.getFinalTransformation (); + transforms_->at (i) = icp_trans * transforms_->at (i); + } + } + + /** + * HYPOTHESES VERIFICATION + **/ + + if (hv_algorithm_) + { + + pcl::ScopeTime thv ("HV verification"); + + std::vector::ConstPtr> aligned_models; + aligned_models.resize (models_->size ()); + for (size_t i = 0; i < models_->size (); i++) + { + ConstPointInTPtr model_cloud = models_->at (i).getAssembled (0.0025f); + //ConstPointInTPtr model_cloud = models_->at (i).getAssembled (VOXEL_SIZE_ICP_); + PointInTPtr model_aligned (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms_->at (i)); + aligned_models[i] = model_aligned; + } + + std::vector mask_hv; + hv_algorithm_->setSceneCloud (input_); + hv_algorithm_->addModels (aligned_models, true); + hv_algorithm_->verify (); + hv_algorithm_->getMask (mask_hv); + + boost::shared_ptr < std::vector > models_temp; + boost::shared_ptr < std::vector > > transforms_temp; + + models_temp.reset (new std::vector); + transforms_temp.reset (new std::vector >); + + for (size_t i = 0; i < models_->size (); i++) + { + if (!mask_hv[i]) + continue; + + models_temp->push_back (models_->at (i)); + transforms_temp->push_back (transforms_->at (i)); + } + + models_ = models_temp; + transforms_ = transforms_temp; + + } + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix) + { + + if (use_cache_) + { + typedef std::pair mv_pair; + mv_pair pair_model_view = std::make_pair (model.id_, view_id); + + std::map, Eigen::aligned_allocator > >::iterator it = + poses_cache_.find (pair_model_view); + + if (it != poses_cache_.end ()) + { + pose_matrix = it->second; + return; + } + + } + + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/pose_" << view_id << ".txt"; + + PersistenceUtils::readMatrixFromFile (dir.str (), pose_matrix); + } + +template class Distance, typename PointInT, typename FeatureT> + void + pcl::rec_3d_framework::LocalRecognitionPipeline::getKeypoints ( + ModelT & model, + int view_id, + typename pcl::PointCloud::Ptr & keypoints_cloud) + { + + if (use_cache_) + { + std::pair pair_model_view = std::make_pair (model.id_, view_id); + typename std::map, PointInTPtr>::iterator it = keypoints_cache_.find (pair_model_view); + + if (it != keypoints_cache_.end ()) + { + keypoints_cloud = it->second; + return; + } + + } + + std::stringstream dir; + std::string path = source_->getModelDescriptorDir (model, training_dir_, descr_name_); + dir << path << "/keypoint_indices_" << view_id << ".pcd"; + + pcl::io::loadPCDFile (dir.str (), *keypoints_cloud); + } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h new file mode 100644 index 00000000..b882dd47 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h @@ -0,0 +1,353 @@ +/* + * local_recognizer.h + * + * Created on: Mar 24, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_LOCAL_RECOGNIZER_H_ +#define REC_FRAMEWORK_LOCAL_RECOGNIZER_H_ + +//#include +#include +#include +#include +#include +#include +#include +#include + +inline bool +correspSorter (const pcl::Correspondence & i, const pcl::Correspondence & j) +{ + return (i.distance < j.distance); +} + +namespace pcl +{ + namespace rec_3d_framework + { + /** + * \brief Object recognition + 6DOF pose based on local features, GC and HV + * Contains keypoints/local features computation, matching using FLANN, + * point-to-point correspondence grouping, pose refinement and hypotheses verification + * Available features: SHOT, FPFH + * See apps/3d_rec_framework/tools/apps for usage + * \author Aitor Aldoma, Federico Tombari + */ + + template class Distance, typename PointInT, typename FeatureT> + class PCL_EXPORTS LocalRecognitionPipeline + { + + typedef typename pcl::PointCloud::Ptr PointInTPtr; + typedef typename pcl::PointCloud::ConstPtr ConstPointInTPtr; + + typedef Distance DistT; + typedef Model ModelT; + + /** \brief Directory where the trained structure will be saved */ + std::string training_dir_; + + /** \brief Point cloud to be classified */ + PointInTPtr input_; + + /** \brief Model data source */ + typename boost::shared_ptr > source_; + + /** \brief Computes a feature */ + typename boost::shared_ptr > estimator_; + + /** \brief Point-to-point correspondence grouping algorithm */ + typename boost::shared_ptr > cg_algorithm_; + + /** \brief Hypotheses verification algorithm */ + typename boost::shared_ptr > hv_algorithm_; + + /** \brief Descriptor name */ + std::string descr_name_; + + /** \brief Id of the model to be used */ + std::string search_model_; + + bool compute_table_plane_; + + class flann_model + { + public: + ModelT model; + int view_id; + int keypoint_id; + std::vector descr; + }; + + flann::Matrix flann_data_; + flann::Index * flann_index_; + std::vector flann_models_; + + std::vector indices_; + + bool use_cache_; + std::map, Eigen::Matrix4f, std::less >, Eigen::aligned_allocator, Eigen::Matrix4f> > > poses_cache_; + std::map, typename pcl::PointCloud::Ptr> keypoints_cache_; + + float threshold_accept_model_hypothesis_; + int ICP_iterations_; + + boost::shared_ptr > models_; + boost::shared_ptr > > transforms_; + + int kdtree_splits_; + float VOXEL_SIZE_ICP_; + + PointInTPtr keypoints_input_; + PointInTPtr processed_; + typename pcl::PointCloud::Ptr signatures_; + + //load features from disk and create flann structure + void + loadFeaturesAndCreateFLANN (); + + inline void + convertToFLANN (const std::vector &models, flann::Matrix &data) + { + data.rows = models.size (); + data.cols = models[0].descr.size (); // number of histogram bins + + flann::Matrix flann_data (new float[models.size () * models[0].descr.size ()], models.size (), models[0].descr.size ()); + + for (size_t i = 0; i < data.rows; ++i) + for (size_t j = 0; j < data.cols; ++j) + { + flann_data.ptr ()[i * data.cols + j] = models[i].descr[j]; + } + + data = flann_data; + } + + void + nearestKSearch (flann::Index * index, const flann_model &model, int k, flann::Matrix &indices, flann::Matrix &distances); + + class ObjectHypothesis + { + public: + ModelT model_; + typename pcl::PointCloud::Ptr correspondences_pointcloud; //points in model coordinates + boost::shared_ptr > feature_distances_; + pcl::CorrespondencesPtr correspondences_to_inputcloud; //indices between correspondences_pointcloud and scene cloud + }; + + void + getPose (ModelT & model, int view_id, Eigen::Matrix4f & pose_matrix); + + void + getKeypoints (ModelT & model, int view_id, typename pcl::PointCloud::Ptr & keypoints_cloud); + + void + drawCorrespondences (PointInTPtr & cloud, ObjectHypothesis & oh, PointInTPtr & keypoints_pointcloud, pcl::Correspondences & correspondences) + { + pcl::visualization::PCLVisualizer vis_corresp_; + vis_corresp_.setWindowName("correspondences..."); + pcl::visualization::PointCloudColorHandlerCustom random_handler (cloud, 255, 0, 0); + vis_corresp_.addPointCloud (cloud, random_handler, "points"); + + typename pcl::PointCloud::ConstPtr cloud_sampled; + cloud_sampled = oh.model_.getAssembled (0.0025f); + + pcl::visualization::PointCloudColorHandlerCustom random_handler_sampled (cloud_sampled, 0, 0, 255); + vis_corresp_.addPointCloud (cloud_sampled, random_handler_sampled, "sampled"); + + for (size_t kk = 0; kk < correspondences.size (); kk++) + { + pcl::PointXYZ p; + p.getVector4fMap () = oh.correspondences_pointcloud->points[correspondences[kk].index_query].getVector4fMap (); + pcl::PointXYZ p_scene; + p_scene.getVector4fMap () = keypoints_pointcloud->points[correspondences[kk].index_match].getVector4fMap (); + + std::stringstream line_name; + line_name << "line_" << kk; + + vis_corresp_.addLine (p_scene, p, line_name.str ()); + } + + vis_corresp_.spin (); + vis_corresp_.removeAllPointClouds(); + vis_corresp_.removeAllShapes(); + vis_corresp_.close(); + } + + public: + + LocalRecognitionPipeline () + { + use_cache_ = false; + threshold_accept_model_hypothesis_ = 0.2f; + ICP_iterations_ = 30; + kdtree_splits_ = 512; + search_model_ = ""; + VOXEL_SIZE_ICP_ = 0.0025f; + compute_table_plane_ = false; + } + + void setISPK(typename pcl::PointCloud::Ptr & signatures, PointInTPtr & p, PointInTPtr & keypoints) + { + keypoints_input_ = keypoints; + signatures_ = signatures; + processed_ = p; + } + + void setVoxelSizeICP(float s) { + VOXEL_SIZE_ICP_ = s; + } + void + setSearchModel (std::string & id) + { + search_model_ = id; + } + + void + setThresholdAcceptHyp (float t) + { + threshold_accept_model_hypothesis_ = t; + } + + ~LocalRecognitionPipeline () + { + + } + + void + setKdtreeSplits (int n) + { + kdtree_splits_ = n; + } + + void + setIndices (std::vector & indices) + { + indices_ = indices; + } + + void + setICPIterations (int it) + { + ICP_iterations_ = it; + } + + void + setUseCache (bool u) + { + use_cache_ = u; + } + + boost::shared_ptr > + getModels () + { + return models_; + } + + boost::shared_ptr > > + getTransforms () + { + return transforms_; + } + + /** + * \brief Sets the model data source_ + */ + void + setDataSource (typename boost::shared_ptr > & source) + { + source_ = source; + } + + typename boost::shared_ptr > + getDataSource () + { + return source_; + } + + /** + * \brief Sets the local feature estimator + */ + void + setFeatureEstimator (typename boost::shared_ptr > & feat) + { + estimator_ = feat; + } + + /** + * \brief Sets the CG algorithm + */ + void + setCGAlgorithm (typename boost::shared_ptr > & alg) + { + cg_algorithm_ = alg; + } + + /** + * \brief Sets the HV algorithm + */ + void + setHVAlgorithm (typename boost::shared_ptr > & alg) + { + hv_algorithm_ = alg; + } + + /** + * \brief Sets the input cloud to be classified + */ + void + setInputCloud (const PointInTPtr & cloud) + { + input_ = cloud; + } + + /** + * \brief Sets the descriptor name + */ + void + setDescriptorName (std::string & name) + { + descr_name_ = name; + } + + /** + * \brief Filesystem dir where to keep the generated training data + */ + void + setTrainingDir (std::string & dir) + { + training_dir_ = dir; + } + + void + setComputeTablePlane(bool b) { + compute_table_plane_ = b; + } + + void + getProcessed(PointInTPtr & cloud) { + cloud = processed_; + } + + /** + * \brief Initializes the FLANN structure from the provided source + * It does training for the models that havent been trained yet + */ + + void + initialize (bool force_retrain = false); + + /** + * \brief Performs recognition and pose estimation on the input cloud + */ + + void + recognize (); + }; + } +} + +#endif /* REC_FRAMEWORK_LOCAL_RECOGNIZER_H_ */ diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/tools/openni_frame_source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/tools/openni_frame_source.h new file mode 100644 index 00000000..1ef7b0c9 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/tools/openni_frame_source.h @@ -0,0 +1,42 @@ +#ifndef OPENNI_CAPTURE_H +#define OPENNI_CAPTURE_H + +#include +#include + +namespace OpenNIFrameSource +{ + + typedef pcl::PointXYZRGBA PointT; + typedef pcl::PointCloud PointCloud; + typedef pcl::PointCloud::Ptr PointCloudPtr; + typedef pcl::PointCloud::ConstPtr PointCloudConstPtr; + + /* A simple class for capturing data from an OpenNI camera */ + class PCL_EXPORTS OpenNIFrameSource + { + public: + OpenNIFrameSource (const std::string& device_id = ""); + ~OpenNIFrameSource (); + + const PointCloudPtr + snap (); + bool + isActive (); + void + onKeyboardEvent (const pcl::visualization::KeyboardEvent & event); + + protected: + void + onNewFrame (const PointCloudConstPtr &cloud); + + pcl::OpenNIGrabber grabber_; + PointCloudPtr most_recent_frame_; + int frame_counter_; + boost::mutex mutex_; + bool active_; + }; + +} + +#endif diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h new file mode 100644 index 00000000..e5186904 --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/metrics.h @@ -0,0 +1,137 @@ +/* + * metrics.h + * + * Created on: Jun 22, 2011 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_METRICS_H_ +#define REC_FRAMEWORK_METRICS_H_ + +#include +#include + +namespace Metrics +{ + using ::std::abs; + + template + struct Accumulator + { + typedef T Type; + }; + + template<> + struct Accumulator + { + typedef float Type; + }; + template<> + struct Accumulator + { + typedef float Type; + }; + template<> + struct Accumulator + { + typedef float Type; + }; + template<> + struct Accumulator + { + typedef float Type; + }; + template<> + struct Accumulator + { + typedef float Type; + }; + template<> + struct Accumulator + { + typedef float Type; + }; + + template + struct HistIntersectionUnionDistance + { + typedef T ElementType; + typedef typename Accumulator::Type ResultType; + + /** + * Compute a distance between two vectors using (1 - (1 + sum(min(a_i,b_i))) / (1 + sum(max(a_i, b_i))) ) + * + * This distance is not a valid kdtree distance, it's not dimensionwise additive + * and ignores worst_dist parameter. + */ + + template + ResultType + operator() (Iterator1 a, Iterator2 b, size_t size, ResultType worst_dist = -1) const + { + (void)worst_dist; + ResultType result = ResultType (); + ResultType min0, min1, min2, min3; + ResultType max0, max1, max2, max3; + Iterator1 last = a + size; + Iterator1 lastgroup = last - 3; + + ResultType sum_min, sum_max; + sum_min = 0; + sum_max = 0; + + while (a < lastgroup) + { + min0 = (a[0] < b[0] ? a[0] : b[0]); + max0 = (a[0] > b[0] ? a[0] : b[0]); + min1 = (a[1] < b[1] ? a[1] : b[1]); + max1 = (a[1] > b[1] ? a[1] : b[1]); + min2 = (a[2] < b[2] ? a[2] : b[2]); + max2 = (a[2] > b[2] ? a[2] : b[2]); + min3 = (a[3] < b[3] ? a[3] : b[3]); + max3 = (a[3] > b[3] ? a[3] : b[3]); + sum_min += min0 + min1 + min2 + min3; + sum_max += max0 + max1 + max2 + max3; + a += 4; + b += 4; + /*if (worst_dist>0 && result>worst_dist) { + return result; + }*/ + } + + while (a < last) + { + min0 = *a < *b ? *a : *b; + max0 = *a > *b ? *a : *b; + sum_min += min0; + sum_max += max0; + a++; + b++; + //std::cout << a << " " << last << std::endl; + } + + result = static_cast (1.0 - ((1 + sum_min) / (1 + sum_max))); + return result; + } + + /* This distance functor is not dimension-wise additive, which + * makes it an invalid kd-tree distance, not implementing the accum_dist method */ + /** + * Partial distance, used by the kd-tree. + */ + template + inline ResultType + accum_dist (const U& a, const V& b, int dim) const + { + //printf("New code being used, accum_dist\n"); + ResultType min0; + ResultType max0; + min0 = (a < b ? a : b) + 1.0; + max0 = (a > b ? a : b) + 1.0; + return (1 - (min0 / max0)); + //return (a+b-2*(a +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + namespace PersistenceUtils + { + + inline bool + writeCentroidToFile (std::string file, Eigen::Vector3f & centroid) + { + std::ofstream out (file.c_str ()); + if (!out) + { + std::cout << "Cannot open file.\n"; + return false; + } + + out << centroid[0] << " " << centroid[1] << " " << centroid[2] << std::endl; + out.close (); + + return true; + } + + inline bool + getCentroidFromFile (std::string file, Eigen::Vector3f & centroid) + { + std::ifstream in; + in.open (file.c_str (), std::ifstream::in); + + char linebuf[256]; + in.getline (linebuf, 256); + std::string line (linebuf); + std::vector < std::string > strs; + boost::split (strs, line, boost::is_any_of (" ")); + centroid[0] = static_cast (atof (strs[0].c_str ())); + centroid[1] = static_cast (atof (strs[1].c_str ())); + centroid[2] = static_cast (atof (strs[2].c_str ())); + + return true; + } + + inline bool + writeMatrixToFile (std::string file, Eigen::Matrix4f & matrix) + { + std::ofstream out (file.c_str ()); + if (!out) + { + std::cout << "Cannot open file.\n"; + return false; + } + + for (size_t i = 0; i < 4; i++) + { + for (size_t j = 0; j < 4; j++) + { + out << matrix (i, j); + if (!(i == 3 && j == 3)) + out << " "; + } + } + out.close (); + + return true; + } + + inline bool + writeFloatToFile (std::string file, float value) + { + std::ofstream out (file.c_str ()); + if (!out) + { + std::cout << "Cannot open file.\n"; + return false; + } + + out << value; + out.close (); + + return true; + } + + inline std::string + getViewId (std::string id) + { + //descriptor_xxx_xx.pcd + //and we want xxx + + std::vector < std::string > strs; + boost::split (strs, id, boost::is_any_of ("_")); + + //std::cout << "id:" << id << std::endl; + //std::cout << "returned:" << strs[strs.size() - 2] << std::endl; + + return strs[strs.size () - 2]; + } + + inline bool + readFloatFromFile (std::string dir, std::string file, float& value) + { + + std::vector < std::string > strs; + boost::split (strs, file, boost::is_any_of ("/")); + + std::string str; + for (size_t i = 0; i < (strs.size () - 1); i++) + { + str += strs[i] + "/"; + } + + std::stringstream matrix_file; + matrix_file << dir << "/" << str << "entropy_" << getViewId (file) << ".txt"; + + std::ifstream in; + in.open (matrix_file.str ().c_str (), std::ifstream::in); + + char linebuf[1024]; + in.getline (linebuf, 1024); + value = static_cast (atof (linebuf)); + + return true; + } + + inline bool + readFloatFromFile (std::string file, float& value) + { + + std::ifstream in; + in.open (file.c_str (), std::ifstream::in); + + char linebuf[1024]; + in.getline (linebuf, 1024); + value = static_cast (atof (linebuf)); + + return true; + } + + inline bool + readMatrixFromFile (std::string dir, std::string file, Eigen::Matrix4f & matrix) + { + + //get the descriptor name from dir + std::vector < std::string > path; + boost::split (path, dir, boost::is_any_of ("/")); + + std::string dname = path[path.size () - 1]; + std::string file_replaced; + for (size_t i = 0; i < (path.size () - 1); i++) + { + file_replaced += path[i] + "/"; + } + + boost::split (path, file, boost::is_any_of ("/")); + std::string id; + + for (size_t i = 0; i < (path.size () - 1); i++) + { + id += path[i]; + if (i < (path.size () - 1)) + { + id += "/"; + } + } + + boost::split (path, file, boost::is_any_of ("/")); + std::string filename = path[path.size () - 1]; + + std::stringstream matrix_file; + matrix_file << file_replaced << id << "/" << dname << "/pose_" << getViewId (file) << ".txt"; + //std::cout << matrix_file.str() << std::endl; + + std::ifstream in; + in.open (matrix_file.str ().c_str (), std::ifstream::in); + + char linebuf[1024]; + in.getline (linebuf, 1024); + std::string line (linebuf); + std::vector < std::string > strs_2; + boost::split (strs_2, line, boost::is_any_of (" ")); + + for (int i = 0; i < 16; i++) + { + matrix (i % 4, i / 4) = static_cast (atof (strs_2[i].c_str ())); + } + + return true; + } + + inline bool + readMatrixFromFile (std::string file, Eigen::Matrix4f & matrix) + { + + std::ifstream in; + in.open (file.c_str (), std::ifstream::in); + + char linebuf[1024]; + in.getline (linebuf, 1024); + std::string line (linebuf); + std::vector < std::string > strs_2; + boost::split (strs_2, line, boost::is_any_of (" ")); + + for (int i = 0; i < 16; i++) + { + matrix (i % 4, i / 4) = static_cast (atof (strs_2[i].c_str ())); + } + + return true; + } + + inline bool + readMatrixFromFile2 (std::string file, Eigen::Matrix4f & matrix) + { + + std::ifstream in; + in.open (file.c_str (), std::ifstream::in); + + char linebuf[1024]; + in.getline (linebuf, 1024); + std::string line (linebuf); + std::vector < std::string > strs_2; + boost::split (strs_2, line, boost::is_any_of (" ")); + + for (int i = 0; i < 16; i++) + { + matrix (i / 4, i % 4) = static_cast (atof (strs_2[i].c_str ())); + } + + return true; + } + + template + inline + void + getPointCloudFromFile (std::string dir, std::string file, typename pcl::PointCloud::Ptr & cloud) + { + + //get the descriptor name from dir + std::vector < std::string > path; + boost::split (path, dir, boost::is_any_of ("/")); + + std::string dname = path[path.size () - 1]; + std::string file_replaced; + for (size_t i = 0; i < (path.size () - 1); i++) + { + file_replaced += path[i] + "/"; + } + + boost::split (path, file, boost::is_any_of ("/")); + std::string id; + + for (size_t i = 0; i < (path.size () - 1); i++) + { + id += path[i]; + if (i < (path.size () - 1)) + { + id += "/"; + } + } + + std::stringstream view_file; + view_file << file_replaced << id << "/" << dname << "/view_" << getViewId (file) << ".pcd"; + + pcl::io::loadPCDFile (view_file.str (), *cloud); + } + } + } +} + diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h new file mode 100644 index 00000000..5860d12c --- /dev/null +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h @@ -0,0 +1,166 @@ +/* + * uniform_sampling.h + * + * Created on: Mar 25, 2012 + * Author: aitor + */ + +#ifndef REC_FRAMEWORK_UNIFORM_SAMPLING_H_ +#define REC_FRAMEWORK_UNIFORM_SAMPLING_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pcl +{ + namespace rec_3d_framework + { + + inline double + uniform_deviate (int seed) + { + double ran = seed * (1.0 / (RAND_MAX + 1.0)); + return ran; + } + + inline void + randomPointTriangle (double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3, Eigen::Vector4f& p) + { + float r1 = static_cast (uniform_deviate (rand ())); + float r2 = static_cast (uniform_deviate (rand ())); + float r1sqr = sqrtf (r1); + float OneMinR1Sqr = (1 - r1sqr); + float OneMinR2 = (1 - r2); + a1 *= OneMinR1Sqr; + a2 *= OneMinR1Sqr; + a3 *= OneMinR1Sqr; + b1 *= OneMinR2; + b2 *= OneMinR2; + b3 *= OneMinR2; + c1 = r1sqr * (r2 * c1 + b1) + a1; + c2 = r1sqr * (r2 * c2 + b2) + a2; + c3 = r1sqr * (r2 * c3 + b3) + a3; + p[0] = static_cast (c1); + p[1] = static_cast (c2); + p[2] = static_cast (c3); + p[3] = 0.f; + } + + inline void + randPSurface (vtkPolyData * polydata, std::vector * cumulativeAreas, double totalArea, Eigen::Vector4f& p) + { + float r = static_cast (uniform_deviate (rand ()) * totalArea); + + std::vector::iterator low = std::lower_bound (cumulativeAreas->begin (), cumulativeAreas->end (), r); + vtkIdType el = static_cast (low - cumulativeAreas->begin ()); + + double A[3], B[3], C[3]; + vtkIdType npts = 0; + vtkIdType *ptIds = NULL; + polydata->GetCellPoints (el, npts, ptIds); + + if (ptIds == NULL) + return; + + polydata->GetPoint (ptIds[0], A); + polydata->GetPoint (ptIds[1], B); + polydata->GetPoint (ptIds[2], C); + randomPointTriangle (A[0], A[1], A[2], B[0], B[1], B[2], C[0], C[1], C[2], p); + } + + template + inline void + uniform_sampling (vtkSmartPointer polydata, size_t n_samples, typename pcl::PointCloud & cloud_out) + { + polydata->BuildCells (); + vtkSmartPointer < vtkCellArray > cells = polydata->GetPolys (); + + double p1[3], p2[3], p3[3], totalArea = 0; + std::vector cumulativeAreas (cells->GetNumberOfCells (), 0); + size_t i = 0; + vtkIdType npts = 0, *ptIds = NULL; + for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds); i++) + { + polydata->GetPoint (ptIds[0], p1); + polydata->GetPoint (ptIds[1], p2); + polydata->GetPoint (ptIds[2], p3); + totalArea += vtkTriangle::TriangleArea (p1, p2, p3); + cumulativeAreas[i] = totalArea; + } + + cloud_out.points.resize (n_samples); + cloud_out.width = static_cast (n_samples); + cloud_out.height = 1; + + for (i = 0; i < n_samples; i++) + { + Eigen::Vector4f p (0.f, 0.f, 0.f, 0.f); + randPSurface (polydata, &cumulativeAreas, totalArea, p); + cloud_out.points[i].x = static_cast (p[0]); + cloud_out.points[i].y = static_cast (p[1]); + cloud_out.points[i].z = static_cast (p[2]); + } + } + + template + inline void + uniform_sampling (std::string & file, size_t n_samples, typename pcl::PointCloud & cloud_out, float scale = 1.f) + { + + vtkSmartPointer < vtkPLYReader > reader = vtkSmartPointer::New (); + reader->SetFileName (file.c_str ()); + + vtkSmartPointer < vtkPolyDataMapper > mapper = vtkSmartPointer::New (); + + if (scale == 1.f) + { + mapper->SetInputConnection (reader->GetOutputPort ()); + mapper->Update (); + } + else + { + vtkSmartPointer trans = vtkSmartPointer::New (); + trans->Scale (scale, scale, scale); + vtkSmartPointer < vtkTransformFilter > trans_filter = vtkSmartPointer::New (); + trans_filter->SetTransform (trans); + trans_filter->SetInputConnection (reader->GetOutputPort ()); + trans_filter->Update (); + mapper->SetInputConnection (trans_filter->GetOutputPort ()); + mapper->Update (); + } + + vtkSmartPointer poly = mapper->GetInput (); + + uniform_sampling (poly, n_samples, cloud_out); + + } + + inline void + getVerticesAsPointCloud (vtkSmartPointer polydata, pcl::PointCloud & cloud_out) + { + vtkPoints *points = polydata->GetPoints (); + cloud_out.points.resize (points->GetNumberOfPoints ()); + cloud_out.width = static_cast (cloud_out.points.size ()); + cloud_out.height = 1; + cloud_out.is_dense = false; + + for (int i = 0; i < points->GetNumberOfPoints (); i++) + { + double p[3]; + points->GetPoint (i, p); + cloud_out.points[i].x = static_cast (p[0]); + cloud_out.points[i].y = static_cast (p[1]); + cloud_out.points[i].z = static_cast (p[2]); + } + } + } +} + +#endif /* REC_FRAMEWORK_UNIFORM_SAMPLING_H_ */ diff --git a/apps/3d_rec_framework/src/pipeline/global_nn_classifier.cpp b/apps/3d_rec_framework/src/pipeline/global_nn_classifier.cpp new file mode 100644 index 00000000..9081fe3f --- /dev/null +++ b/apps/3d_rec_framework/src/pipeline/global_nn_classifier.cpp @@ -0,0 +1,16 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include "pcl/apps/3d_rec_framework/utils/metrics.h" + +//Instantiation +template class pcl::rec_3d_framework::GlobalNNPipeline; +template class pcl::rec_3d_framework::GlobalNNPipeline; +template class pcl::rec_3d_framework::GlobalNNPipeline; + +template class pcl::rec_3d_framework::GlobalClassifier; diff --git a/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_crh.cpp b/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_crh.cpp new file mode 100644 index 00000000..8ca6d2b5 --- /dev/null +++ b/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_crh.cpp @@ -0,0 +1,21 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include "pcl/apps/3d_rec_framework/utils/metrics.h" + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<90>, + (float[90], histogram, histogram90) +) + +//Instantiation +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCRHRecognizer; diff --git a/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_cvfh.cpp b/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_cvfh.cpp new file mode 100644 index 00000000..86b6037f --- /dev/null +++ b/apps/3d_rec_framework/src/pipeline/global_nn_recognizer_cvfh.cpp @@ -0,0 +1,48 @@ +/* + * global_nn_classifier.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include "pcl/apps/3d_rec_framework/utils/metrics.h" + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<367>, + (float[367], histogram, histogram367) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<431>, + (float[431], histogram, histogram431) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<559>, + (float[559], histogram, histogram559) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<815>, + (float[815], histogram, histogram815) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<879>, + (float[879], histogram, histogram879) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<1327>, + (float[1327], histogram, histogram1327) +) + + +//Instantiation +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer; +template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; +//template class pcl::rec_3d_framework::GlobalNNCVFHRecognizer >; diff --git a/apps/3d_rec_framework/src/pipeline/local_recognizer.cpp b/apps/3d_rec_framework/src/pipeline/local_recognizer.cpp new file mode 100644 index 00000000..8006a8b3 --- /dev/null +++ b/apps/3d_rec_framework/src/pipeline/local_recognizer.cpp @@ -0,0 +1,22 @@ +#include + +//This stuff is needed to be able to make the SHOT histograms persistent +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<352>, + (float[352], histogram, histogram352) +) + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::Histogram<1344>, + (float[1344], histogram, histogram1344) +) + +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline; + +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline >; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline; +template class PCL_EXPORTS pcl::rec_3d_framework::LocalRecognitionPipeline; diff --git a/apps/3d_rec_framework/src/tools/openni_frame_source.cpp b/apps/3d_rec_framework/src/tools/openni_frame_source.cpp new file mode 100644 index 00000000..4bf10947 --- /dev/null +++ b/apps/3d_rec_framework/src/tools/openni_frame_source.cpp @@ -0,0 +1,57 @@ +#include "pcl/apps/3d_rec_framework/tools/openni_frame_source.h" +#include +#include +#include + +namespace OpenNIFrameSource +{ + + OpenNIFrameSource::OpenNIFrameSource (const std::string& device_id) : + grabber_ (device_id), most_recent_frame_ (), frame_counter_ (0), active_ (true) + { + boost::function frame_cb = boost::bind (&OpenNIFrameSource::onNewFrame, this, _1); + grabber_.registerCallback (frame_cb); + grabber_.start (); + } + + OpenNIFrameSource::~OpenNIFrameSource () + { + // Stop the grabber when shutting down + grabber_.stop (); + } + + bool + OpenNIFrameSource::isActive () + { + return active_; + } + + const PointCloudPtr + OpenNIFrameSource::snap () + { + return (most_recent_frame_); + } + + void + OpenNIFrameSource::onNewFrame (const PointCloudConstPtr &cloud) + { + mutex_.lock (); + ++frame_counter_; + most_recent_frame_ = boost::make_shared (*cloud); // Make a copy of the frame + mutex_.unlock (); + } + + void + OpenNIFrameSource::onKeyboardEvent (const pcl::visualization::KeyboardEvent & event) + { + // When the spacebar is pressed, trigger a frame capture + mutex_.lock (); + if (event.keyDown () && event.getKeySym () == "e") + { + active_ = false; + } + mutex_.unlock (); + } + +} diff --git a/apps/3d_rec_framework/tools/CMakeLists.txt b/apps/3d_rec_framework/tools/CMakeLists.txt new file mode 100644 index 00000000..abb140ad --- /dev/null +++ b/apps/3d_rec_framework/tools/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(apps) \ No newline at end of file diff --git a/apps/3d_rec_framework/tools/apps/CMakeLists.txt b/apps/3d_rec_framework/tools/apps/CMakeLists.txt new file mode 100644 index 00000000..4862fb37 --- /dev/null +++ b/apps/3d_rec_framework/tools/apps/CMakeLists.txt @@ -0,0 +1,10 @@ +if (QHULL_FOUND) + add_executable(pcl_global_classification src/global_classification.cpp) + target_link_libraries(pcl_global_classification pcl_apps pcl_3d_rec_framework pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface) +endif() + +add_executable(pcl_local_or_mian src/local_recognition_mian_dataset.cpp) +target_link_libraries(pcl_local_or_mian pcl_apps pcl_3d_rec_framework pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints) + +#add_executable(pcl_local_face src/local_recognition_faces.cpp) +#target_link_libraries(pcl_local_face pcl_apps pcl_3d_rec_framework pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints) diff --git a/apps/3d_rec_framework/tools/apps/src/global_classification.cpp b/apps/3d_rec_framework/tools/apps/src/global_classification.cpp new file mode 100644 index 00000000..e21f1f43 --- /dev/null +++ b/apps/3d_rec_framework/tools/apps/src/global_classification.cpp @@ -0,0 +1,231 @@ +/* + * test_training.cpp + * + * Created on: Mar 9, 2012 + * Author: aitor + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +template class DistT, typename PointT, typename FeatureT> +void +segmentAndClassify (typename pcl::rec_3d_framework::GlobalNNPipeline & global) +{ + //get point cloud from the kinect, segment it and classify it + OpenNIFrameSource::OpenNIFrameSource camera; + OpenNIFrameSource::PointCloudPtr frame; + + pcl::visualization::PCLVisualizer vis ("kinect"); + + //keyboard callback to stop getting frames and finalize application + boost::function keyboard_cb = boost::bind (&OpenNIFrameSource::OpenNIFrameSource::onKeyboardEvent, &camera, _1); + vis.registerKeyboardCallback (keyboard_cb); + size_t previous_cluster_size = 0; + size_t previous_categories_size = 0; + + float Z_DIST_ = 1.25f; + float text_scale = 0.015f; + + while (camera.isActive ()) + { + pcl::ScopeTime frame_process ("Global frame processing ------------- "); + frame = camera.snap (); + + pcl::PointCloud::Ptr xyz_points (new pcl::PointCloud); + pcl::copyPointCloud (*frame, *xyz_points); + + //Step 1 -> Segment + pcl::apps::DominantPlaneSegmentation dps; + dps.setInputCloud (xyz_points); + dps.setMaxZBounds (Z_DIST_); + dps.setObjectMinHeight (0.005); + dps.setMinClusterSize (1000); + dps.setWSize (9); + dps.setDistanceBetweenClusters (0.1f); + + std::vector::Ptr> clusters; + std::vector indices; + dps.setDownsamplingSize (0.02f); + dps.compute_fast (clusters); + dps.getIndicesClusters (indices); + Eigen::Vector4f table_plane_; + Eigen::Vector3f normal_plane_ = Eigen::Vector3f (table_plane_[0], table_plane_[1], table_plane_[2]); + dps.getTableCoefficients (table_plane_); + + vis.removePointCloud ("frame"); + vis.addPointCloud (frame, "frame"); + + for (size_t i = 0; i < previous_cluster_size; i++) + { + std::stringstream cluster_name; + cluster_name << "cluster_" << i; + vis.removePointCloud (cluster_name.str ()); + + cluster_name << "_ply_model_"; + vis.removeShape (cluster_name.str ()); + } + + for (size_t i = 0; i < previous_categories_size; i++) + { + std::stringstream cluster_text; + cluster_text << "cluster_" << i << "_text"; + vis.removeText3D (cluster_text.str ()); + } + + previous_categories_size = 0; + float dist_ = 0.03f; + + for (size_t i = 0; i < clusters.size (); i++) + { + std::stringstream cluster_name; + cluster_name << "cluster_" << i; + pcl::visualization::PointCloudColorHandlerRandom random_handler (clusters[i]); + vis.addPointCloud (clusters[i], random_handler, cluster_name.str ()); + + global.setInputCloud (xyz_points); + global.setIndices (indices[i].indices); + global.classify (); + + std::vector < std::string > categories; + std::vector conf; + + global.getCategory (categories); + global.getConfidence (conf); + + std::string category = categories[0]; + Eigen::Vector4f centroid; + pcl::compute3DCentroid (*xyz_points, indices[i].indices, centroid); + for (size_t kk = 0; kk < categories.size (); kk++) + { + + pcl::PointXYZ pos; + pos.x = centroid[0] + normal_plane_[0] * static_cast (kk + 1) * dist_; + pos.y = centroid[1] + normal_plane_[1] * static_cast (kk + 1) * dist_; + pos.z = centroid[2] + normal_plane_[2] * static_cast (kk + 1) * dist_; + + std::ostringstream prob_str; + prob_str.precision (1); + prob_str << categories[kk] << " [" << conf[kk] << "]"; + + std::stringstream cluster_text; + cluster_text << "cluster_" << previous_categories_size << "_text"; + vis.addText3D (prob_str.str (), pos, text_scale, 1, 0, 1, cluster_text.str (), 0); + previous_categories_size++; + } + } + + previous_cluster_size = clusters.size (); + + vis.spinOnce (); + } +} + +//bin/pcl_global_classification -models_dir /home/aitor/data/3d-net_one_class/ -descriptor_name esf -training_dir /home/aitor/data/3d-net_one_class_trained_level_1 -nn 10 + +int +main (int argc, char ** argv) +{ + + std::string path = "models/"; + std::string desc_name = "esf"; + std::string training_dir = "trained_models/"; + int NN = 1; + + pcl::console::parse_argument (argc, argv, "-models_dir", path); + pcl::console::parse_argument (argc, argv, "-training_dir", training_dir); + pcl::console::parse_argument (argc, argv, "-descriptor_name", desc_name); + pcl::console::parse_argument (argc, argv, "-nn", NN); + + //pcl::console::parse_argument (argc, argv, "-z_dist", chop_at_z_); + //pcl::console::parse_argument (argc, argv, "-tesselation_level", views_level_); + + boost::shared_ptr > mesh_source (new pcl::rec_3d_framework::MeshSource); + mesh_source->setPath (path); + mesh_source->setResolution (150); + mesh_source->setTesselationLevel (1); + mesh_source->setViewAngle (57.f); + mesh_source->setRadiusSphere (1.5f); + mesh_source->setModelScale (1.f); + mesh_source->generate (training_dir); + + boost::shared_ptr > cast_source; + cast_source = boost::static_pointer_cast > (mesh_source); + + boost::shared_ptr > normal_estimator; + normal_estimator.reset (new pcl::rec_3d_framework::PreProcessorAndNormalEstimator); + normal_estimator->setCMR (true); + normal_estimator->setDoVoxelGrid (true); + normal_estimator->setRemoveOutliers (true); + normal_estimator->setFactorsForCMR (3, 7); + + if (desc_name.compare ("vfh") == 0) + { + boost::shared_ptr > vfh_estimator; + vfh_estimator.reset (new pcl::rec_3d_framework::VFHEstimation); + vfh_estimator->setNormalEstimator (normal_estimator); + + boost::shared_ptr > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > (vfh_estimator); + + pcl::rec_3d_framework::GlobalNNPipeline global; + global.setDataSource (cast_source); + global.setTrainingDir (training_dir); + global.setDescriptorName (desc_name); + global.setNN (NN); + global.setFeatureEstimator (cast_estimator); + global.initialize (true); + + segmentAndClassify (global); + } + + if (desc_name.compare ("cvfh") == 0) + { + boost::shared_ptr > vfh_estimator; + vfh_estimator.reset (new pcl::rec_3d_framework::CVFHEstimation); + vfh_estimator->setNormalEstimator (normal_estimator); + + boost::shared_ptr > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > (vfh_estimator); + + pcl::rec_3d_framework::GlobalNNPipeline global; + global.setDataSource (cast_source); + global.setTrainingDir (training_dir); + global.setDescriptorName (desc_name); + global.setFeatureEstimator (cast_estimator); + global.setNN (NN); + global.initialize (false); + + segmentAndClassify (global); + } + + if (desc_name.compare ("esf") == 0) + { + boost::shared_ptr > estimator; + estimator.reset (new pcl::rec_3d_framework::ESFEstimation); + + boost::shared_ptr > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > (estimator); + + pcl::rec_3d_framework::GlobalNNPipeline global; + global.setDataSource (cast_source); + global.setTrainingDir (training_dir); + global.setDescriptorName (desc_name); + global.setFeatureEstimator (cast_estimator); + global.setNN (NN); + global.initialize (false); + + segmentAndClassify (global); + } + +} diff --git a/apps/3d_rec_framework/tools/apps/src/local_recognition_mian_dataset.cpp b/apps/3d_rec_framework/tools/apps/src/local_recognition_mian_dataset.cpp new file mode 100644 index 00000000..4bed483a --- /dev/null +++ b/apps/3d_rec_framework/tools/apps/src/local_recognition_mian_dataset.cpp @@ -0,0 +1,551 @@ +/* + * local_recognition_mian_dataset.cpp + * + * Created on: Mar 24, 2012 + * Author: aitor + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void +getScenesInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector & relative_paths) +{ + //list models in MODEL_FILES_DIR_ and return list + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (bf::is_directory (*itr)) + { + +#if BOOST_FILESYSTEM_VERSION == 3 + std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/"; +#else + std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/"; +#endif + bf::path curr_path = itr->path (); + getScenesInDirectory (curr_path, so_far, relative_paths); + } + else + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ().filename ()); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + std::string extension = strs[strs.size () - 1]; + + if (extension == "pcd") + { + +#if BOOST_FILESYSTEM_VERSION == 3 + std::string path = rel_path_so_far + (itr->path ().filename ()).string(); +#else + std::string path = rel_path_so_far + (itr->path ()).filename (); +#endif + + relative_paths.push_back (path); + } + } + } +} + +inline bool +sortFiles (const std::string & file1, const std::string & file2) +{ + std::vector < std::string > strs1; + boost::split (strs1, file1, boost::is_any_of ("/")); + + std::vector < std::string > strs2; + boost::split (strs2, file2, boost::is_any_of ("/")); + + std::string id_1 = strs1[strs1.size () - 1]; + std::string id_2 = strs2[strs2.size () - 1]; + + size_t pos1 = id_1.find (".ply.pcd"); + size_t pos2 = id_2.find (".ply.pcd"); + + id_1 = id_1.substr (0, pos1); + id_2 = id_2.substr (0, pos2); + + id_1 = id_1.substr (2); + id_2 = id_2.substr (2); + + return atoi (id_1.c_str ()) < atoi (id_2.c_str ()); +} + +template class DistT, typename PointT, typename FeatureT> + void + recognizeAndVisualize (typename pcl::rec_3d_framework::LocalRecognitionPipeline & local, std::string & scenes_dir, + int scene = -1, bool single_model = false) + { + + //read mians scenes + bf::path ply_files_dir = scenes_dir; + std::vector < std::string > files; + std::string start = ""; + getScenesInDirectory (ply_files_dir, start, files); + + std::sort (files.begin (), files.end (), sortFiles); + + typename boost::shared_ptr > model_source_ = local.getDataSource (); + typedef typename pcl::PointCloud::ConstPtr ConstPointInTPtr; + typedef pcl::rec_3d_framework::Model ModelT; + + if (!single_model) + { + pcl::visualization::PCLVisualizer vis ("Mians dataset"); + + for (size_t i = 0; i < files.size (); i++) + { + std::cout << files[i] << std::endl; + if (scene != -1) + if (scene != i) + continue; + + std::stringstream file; + file << ply_files_dir.string () << files[i]; + + typename pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); + pcl::io::loadPCDFile (file.str (), *scene); + + local.setVoxelSizeICP (0.005f); + local.setInputCloud (scene); + { + pcl::ScopeTime ttt ("Recognition"); + local.recognize (); + } + + std::stringstream scene_name; + scene_name << "Scene " << (i + 1); + vis.addPointCloud (scene, "scene_cloud"); + vis.addText (scene_name.str (), 1, 30, 24, 1, 0, 0, "scene_text"); + + //visualize results + boost::shared_ptr < std::vector > models = local.getModels (); + boost::shared_ptr < std::vector > > transforms = local.getTransforms (); + + for (size_t j = 0; j < models->size (); j++) + { + std::stringstream name; + name << "cloud_" << j; + + ConstPointInTPtr model_cloud = models->at (j).getAssembled (0.0025f); + typename pcl::PointCloud::Ptr model_aligned (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms->at (j)); + + float r, g, b; + std::cout << models->at (j).id_ << std::endl; + r = 255.0f; + g = 0.0f; + b = 0.0f; + + if (models->at (j).id_.compare ("cheff") == 0) + { + r = 0.0f; + g = 255.0f; + b = 0.0f; + } + else if (models->at (j).id_.compare ("chicken_high") == 0) + { + r = 0.0f; + g = 255.0f; + b = 255.0f; + } + else if (models->at (j).id_.compare ("parasaurolophus_high") == 0) + { + r = 255.0f; + g = 255.0f; + b = 0.f; + } + else + { + + } + + pcl::visualization::PointCloudColorHandlerCustom random_handler (model_aligned, r, g, b); + vis.addPointCloud (model_aligned, random_handler, name.str ()); + } + + vis.spin (); + + vis.removePointCloud ("scene_cloud"); + vis.removeShape ("scene_text"); + for (size_t j = 0; j < models->size (); j++) + { + std::stringstream name; + name << "cloud_" << j; + vis.removePointCloud (name.str ()); + } + } + } + + if (single_model) + { + //some applications prefer to search for a single object only + std::string id = "chicken_high"; + pcl::visualization::PCLVisualizer vis ("Single model - chicken"); + local.setSearchModel (id); + local.initialize (); + + for (size_t i = 0; i < files.size (); i++) + { + std::stringstream file; + file << ply_files_dir.string () << files[i]; + + typename pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); + pcl::io::loadPCDFile (file.str (), *scene); + + local.setInputCloud (scene); + local.recognize (); + + std::stringstream scene_name; + scene_name << "Scene " << (i + 1); + vis.addPointCloud (scene, "scene_cloud"); + vis.addText (scene_name.str (), 1, 30, 24, 1, 0, 0, "scene_text"); + + //visualize results + boost::shared_ptr < std::vector > models = local.getModels (); + boost::shared_ptr < std::vector > > transforms = local.getTransforms (); + + for (size_t j = 0; j < models->size (); j++) + { + std::stringstream name; + name << "cloud_" << j; + + ConstPointInTPtr model_cloud = models->at (j).getAssembled (0.0025f); + typename pcl::PointCloud::Ptr model_aligned (new pcl::PointCloud); + pcl::transformPointCloud (*model_cloud, *model_aligned, transforms->at (j)); + + pcl::visualization::PointCloudColorHandlerRandom random_handler (model_aligned); + vis.addPointCloud (model_aligned, random_handler, name.str ()); + } + + vis.spin (); + + vis.removePointCloud ("scene_cloud"); + vis.removeShape ("scene_text"); + for (size_t j = 0; j < models->size (); j++) + { + std::stringstream name; + name << "cloud_" << j; + vis.removePointCloud (name.str ()); + } + } + } + } + +void +getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector & relative_paths, std::string & ext) +{ + bf::directory_iterator end_itr; + for (bf::directory_iterator itr (dir); itr != end_itr; ++itr) + { + //check if its a directory, then get models in it + if (bf::is_directory (*itr)) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string so_far = rel_path_so_far + (itr->path ().filename ()).string() + "/"; +#else + std::string so_far = rel_path_so_far + (itr->path ()).filename () + "/"; +#endif + + bf::path curr_path = itr->path (); + getModelsInDirectory (curr_path, so_far, relative_paths, ext); + } + else + { + //check that it is a ply file and then add, otherwise ignore.. + std::vector < std::string > strs; +#if BOOST_FILESYSTEM_VERSION == 3 + std::string file = (itr->path ().filename ()).string(); +#else + std::string file = (itr->path ()).filename (); +#endif + + boost::split (strs, file, boost::is_any_of (".")); + std::string extension = strs[strs.size () - 1]; + + if (extension.compare (ext) == 0) + { +#if BOOST_FILESYSTEM_VERSION == 3 + std::string path = rel_path_so_far + (itr->path ().filename ()).string(); +#else + std::string path = rel_path_so_far + (itr->path ()).filename (); +#endif + + relative_paths.push_back (path); + } + } + } +} + +typedef pcl::ReferenceFrame RFType; + +int CG_SIZE_ = 3; +float CG_THRESHOLD_ = 0.005f; + +/** Based on the paper: + * "A Global Hypotheses Verification Method for 3D Object Recognition", + * A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012 + * + * Note: Due to changes PCL experimented between submission and the current status, + * you might find some inconsistencies between parameter value in code and in the paper. + * (tested on revision 7453) + */ + +int +main (int argc, char ** argv) +{ + std::string path = ""; + std::string desc_name = "shot_omp"; + std::string training_dir = "trained_models/"; + std::string mians_scenes = ""; + int force_retrain = 0; + int icp_iterations = 20; + int use_cache = 1; + int splits = 512; + int scene = -1; + int detect_clutter = 1; + int hv_method = 0; + int use_hv = 1; + float thres_hyp_ = 0.2f; + float desc_radius = 0.04f; + + pcl::console::parse_argument (argc, argv, "-models_dir", path); + pcl::console::parse_argument (argc, argv, "-training_dir", training_dir); + pcl::console::parse_argument (argc, argv, "-descriptor_name", desc_name); + pcl::console::parse_argument (argc, argv, "-mians_scenes_dir", mians_scenes); + pcl::console::parse_argument (argc, argv, "-force_retrain", force_retrain); + pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations); + pcl::console::parse_argument (argc, argv, "-use_cache", use_cache); + pcl::console::parse_argument (argc, argv, "-splits", splits); + pcl::console::parse_argument (argc, argv, "-gc_size", CG_SIZE_); + pcl::console::parse_argument (argc, argv, "-gc_threshold", CG_THRESHOLD_); + pcl::console::parse_argument (argc, argv, "-scene", scene); + pcl::console::parse_argument (argc, argv, "-detect_clutter", detect_clutter); + pcl::console::parse_argument (argc, argv, "-hv_method", hv_method); + pcl::console::parse_argument (argc, argv, "-use_hv", use_hv); + pcl::console::parse_argument (argc, argv, "-thres_hyp", thres_hyp_); + + if (mians_scenes.compare ("") == 0) + { + PCL_ERROR("Set the directory containing mians scenes using the -mians_scenes_dir [dir] option\n"); + return -1; + } + + if (path.compare ("") == 0) + { + PCL_ERROR("Set the directory containing the models of mian dataset using the -models_dir [dir] option\n"); + return -1; + } + + bf::path models_dir_path = path; + if (!bf::exists (models_dir_path)) + { + PCL_ERROR("Models dir path %s does not exist, use -models_dir [dir] option\n", path.c_str()); + return -1; + } + else + { + std::vector < std::string > files; + std::string start = ""; + std::string ext = std::string ("ply"); + bf::path dir = models_dir_path; + getModelsInDirectory (dir, start, files, ext); + assert (files.size () == 4); + } + + //configure mesh source + boost::shared_ptr > mesh_source (new pcl::rec_3d_framework::MeshSource); + mesh_source->setPath (path); + mesh_source->setResolution (250); + mesh_source->setTesselationLevel (1); + mesh_source->setViewAngle (57.f); + mesh_source->setRadiusSphere (1.5f); + mesh_source->setModelScale (0.001f); + mesh_source->generate (training_dir); + + boost::shared_ptr > cast_source; + cast_source = boost::static_pointer_cast > (mesh_source); + + //configure normal estimator + boost::shared_ptr > normal_estimator; + normal_estimator.reset (new pcl::rec_3d_framework::PreProcessorAndNormalEstimator); + normal_estimator->setCMR (false); + normal_estimator->setDoVoxelGrid (true); + normal_estimator->setRemoveOutliers (true); + normal_estimator->setValuesForCMRFalse (0.003f, 0.012f); + + //configure keypoint extractor + boost::shared_ptr > + uniform_keypoint_extractor ( + new pcl::rec_3d_framework::UniformSamplingExtractor< + pcl::PointXYZ>); + //uniform_keypoint_extractor->setSamplingDensity (0.01f); + uniform_keypoint_extractor->setSamplingDensity (0.005f); + uniform_keypoint_extractor->setFilterPlanar (true); + + boost::shared_ptr > keypoint_extractor; + keypoint_extractor = boost::static_pointer_cast > (uniform_keypoint_extractor); + + //configure cg algorithm (geometric consistency grouping) + boost::shared_ptr > cast_cg_alg; + boost::shared_ptr > gcg_alg ( + new pcl::GeometricConsistencyGrouping); + gcg_alg->setGCThreshold (CG_SIZE_); + gcg_alg->setGCSize (CG_THRESHOLD_); + cast_cg_alg = boost::static_pointer_cast > (gcg_alg); + + //configure hypothesis verificator + boost::shared_ptr > papazov (new pcl::PapazovHV); + papazov->setResolution (0.005f); + papazov->setInlierThreshold (0.005f); + papazov->setSupportThreshold (0.08f); + papazov->setPenaltyThreshold (0.05f); + papazov->setConflictThreshold (0.02f); + papazov->setOcclusionThreshold (0.01f); + + boost::shared_ptr > go ( + new pcl::GlobalHypothesesVerification); + go->setResolution (0.005f); + go->setMaxIterations (7000); + go->setInlierThreshold (0.005f); + go->setRadiusClutter (0.04f); + go->setRegularizer (3.f); + go->setClutterRegularizer (7.5f); + go->setDetectClutter (detect_clutter); + go->setOcclusionThreshold (0.01f); + + boost::shared_ptr > greedy (new pcl::GreedyVerification (3.f)); + greedy->setResolution (0.005f); + greedy->setInlierThreshold (0.005f); + greedy->setOcclusionThreshold (0.01f); + + boost::shared_ptr > cast_hv_alg; + + switch (hv_method) + { + case 1: + cast_hv_alg = boost::static_pointer_cast > (greedy); + break; + case 2: + cast_hv_alg = boost::static_pointer_cast > (papazov); + break; + default: + cast_hv_alg = boost::static_pointer_cast > (go); + } + + if (desc_name.compare ("shot") == 0) + { + boost::shared_ptr > > estimator; + estimator.reset (new pcl::rec_3d_framework::SHOTLocalEstimation >); + estimator->setNormalEstimator (normal_estimator); + estimator->addKeypointExtractor (keypoint_extractor); + estimator->setSupportRadius (0.04f); + + boost::shared_ptr > > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > > (estimator); + + pcl::rec_3d_framework::LocalRecognitionPipeline > local; + local.setDataSource (cast_source); + local.setTrainingDir (training_dir); + local.setDescriptorName (desc_name); + local.setFeatureEstimator (cast_estimator); + local.setCGAlgorithm (cast_cg_alg); + if (use_hv) + local.setHVAlgorithm (cast_hv_alg); + local.setUseCache (static_cast (use_cache)); + local.initialize (static_cast (force_retrain)); + + uniform_keypoint_extractor->setSamplingDensity (0.005f); + local.setICPIterations (icp_iterations); + local.setKdtreeSplits (splits); + + recognizeAndVisualize > (local, mians_scenes); + + } + + if (desc_name.compare ("shot_omp") == 0) + { + desc_name = std::string ("shot"); + boost::shared_ptr > > estimator; + estimator.reset (new pcl::rec_3d_framework::SHOTLocalEstimationOMP >); + estimator->setNormalEstimator (normal_estimator); + estimator->addKeypointExtractor (keypoint_extractor); + //estimator->setSupportRadius (0.04f); + estimator->setSupportRadius (desc_radius); + + boost::shared_ptr > > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > > (estimator); + + pcl::rec_3d_framework::LocalRecognitionPipeline > local; + local.setDataSource (cast_source); + local.setTrainingDir (training_dir); + local.setDescriptorName (desc_name); + local.setFeatureEstimator (cast_estimator); + local.setCGAlgorithm (cast_cg_alg); + if (use_hv) + local.setHVAlgorithm (cast_hv_alg); + + local.setUseCache (static_cast (use_cache)); + local.initialize (static_cast (force_retrain)); + local.setThresholdAcceptHyp (thres_hyp_); + + uniform_keypoint_extractor->setSamplingDensity (0.005f); + local.setICPIterations (icp_iterations); + local.setKdtreeSplits (splits); + + recognizeAndVisualize > (local, mians_scenes, scene); + + } + + if (desc_name.compare ("fpfh") == 0) + { + boost::shared_ptr > estimator; + estimator.reset (new pcl::rec_3d_framework::FPFHLocalEstimation); + estimator->setNormalEstimator (normal_estimator); + estimator->addKeypointExtractor (keypoint_extractor); + estimator->setSupportRadius (0.04f); + + boost::shared_ptr > cast_estimator; + cast_estimator = boost::dynamic_pointer_cast > (estimator); + + pcl::rec_3d_framework::LocalRecognitionPipeline local; + local.setDataSource (cast_source); + local.setTrainingDir (training_dir); + local.setDescriptorName (desc_name); + local.setFeatureEstimator (cast_estimator); + local.setCGAlgorithm (cast_cg_alg); + if (use_hv) + local.setHVAlgorithm (cast_hv_alg); + + local.setUseCache (static_cast (use_cache)); + local.initialize (static_cast (force_retrain)); + + uniform_keypoint_extractor->setSamplingDensity (0.005f); + local.setICPIterations (icp_iterations); + local.setKdtreeSplits (splits); + + recognizeAndVisualize (local, mians_scenes); + } +} diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 93b11e97..ff18a65c 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -1,25 +1,6 @@ set(SUBSYS_NAME apps) set(SUBSYS_DESC "Application examples/samples that show how PCL works") -set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition) - -# Find VTK -if(NOT VTK_FOUND) - set(DEFAULT FALSE) - set(REASON "VTK was not found.") -else(NOT VTK_FOUND) - set(DEFAULT TRUE) - set(REASON) - include("${VTK_USE_FILE}") -endif(NOT VTK_FOUND) - -# OpenNI found? -if(NOT OPENNI_FOUND) - set(DEFAULT FALSE) - set(REASON "OpenNI was not found.") -else(NOT OPENNI_FOUND) - set(DEFAULT TRUE) - set(REASON) -endif(NOT OPENNI_FOUND) +set(SUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search recognition ml stereo) set(DEFAULT FALSE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") @@ -47,6 +28,12 @@ if(build) endif(LIBUSB_1_FOUND) if (VTK_FOUND) + + include("${VTK_USE_FILE}") + + set(incs "include/pcl/${SUBSYS_NAME}/render_views_tesselated_sphere.h") + set(srcs "src/render_views_tesselated_sphere.cpp") + PCL_ADD_EXECUTABLE(pcl_ppf_object_recognition "${SUBSYS_NAME}" src/ppf_object_recognition.cpp) target_link_libraries(pcl_ppf_object_recognition pcl_common pcl_io pcl_filters pcl_features pcl_registration pcl_visualization pcl_sample_consensus pcl_segmentation) @@ -73,6 +60,18 @@ if(build) # PCL_ADD_EXECUTABLE(pcl_convolve "${SUBSYS_NAME}" src/convolve.cpp) # target_link_libraries(pcl_convolve pcl_common pcl_io pcl_visualization) + PCL_ADD_EXECUTABLE(pcl_pcd_organized_edge_detection "${SUBSYS_NAME}" src/pcd_organized_edge_detection.cpp) + target_link_libraries(pcl_pcd_organized_edge_detection pcl_common pcl_io pcl_features pcl_visualization) + + PCL_ADD_EXECUTABLE(pcl_face_trainer "${SUBSYS_NAME}" src/face_detection/face_trainer.cpp) + target_link_libraries(pcl_face_trainer pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES}) + + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_fs_face_detector "${SUBSYS_NAME}" src/face_detection//filesystem_face_detection.cpp) + target_link_libraries(pcl_fs_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES}) + + PCL_ADD_EXECUTABLE(pcl_stereo_ground_segmentation "${SUBSYS_NAME}" src/stereo_ground_segmentation.cpp) + target_link_libraries(pcl_stereo_ground_segmentation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_features pcl_stereo) + if (QT4_FOUND AND VTK_USE_QVTK) # Manual registration demo @@ -88,7 +87,7 @@ if(build) endif (QT4_FOUND AND VTK_USE_QVTK) - if (OPENNI_FOUND AND BUILD_OPENNI) + if (WITH_OPENNI) # PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_grab_frame "${SUBSYS_NAME}" src/openni_grab_frame.cpp) # target_link_libraries(pcl_openni_grab_frame pcl_common pcl_io pcl_visualization) @@ -141,6 +140,12 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_boundary_estimation "${SUBSYS_NAME}" src/openni_boundary_estimation.cpp) target_link_libraries(pcl_openni_boundary_estimation pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_organized_edge_detection "${SUBSYS_NAME}" src/openni_organized_edge_detection.cpp) + target_link_libraries(pcl_openni_organized_edge_detection pcl_common pcl_io pcl_features pcl_visualization) + + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_face_detector "${SUBSYS_NAME}" src/face_detection//openni_face_detection.cpp src/face_detection//openni_frame_source.cpp) + target_link_libraries(pcl_openni_face_detector pcl_features pcl_recognition pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface pcl_keypoints pcl_ml pcl_search pcl_kdtree ${VTK_LIBRARIES}) + if (QT4_FOUND AND VTK_USE_QVTK) # OpenNI Passthrough application demo QT4_WRAP_UI(openni_passthrough_ui src/openni_passthrough.ui) @@ -162,11 +167,6 @@ if(build) endif () - set(incs - include/pcl/${SUBSYS_NAME}/render_views_tesselated_sphere.h - include/pcl/${SUBSYS_NAME}/timer.h) - set(srcs src/render_views_tesselated_sphere.cpp) - if (QHULL_FOUND) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_3d_convex_hull "${SUBSYS_NAME}" src/openni_3d_convex_hull.cpp) target_link_libraries(pcl_openni_3d_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_surface) @@ -177,10 +177,6 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_tracking "${SUBSYS_NAME}" src/openni_tracking.cpp) target_link_libraries(pcl_openni_tracking pcl_common pcl_io pcl_surface pcl_visualization pcl_filters pcl_features pcl_segmentation pcl_tracking pcl_search) - set(incs "include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h" ${incs}) - set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp") - set(srcs src/dominant_plane_segmentation.cpp ${srcs}) - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_planar_convex_hull "${SUBSYS_NAME}" src/openni_planar_convex_hull.cpp) target_link_libraries(pcl_openni_planar_convex_hull pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_surface) @@ -189,20 +185,12 @@ if(build) endif() # QHULL_FOUND - # Install include files - PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) - PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) - - set(LIB_NAME "pcl_${SUBSYS_NAME}") - PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${impl_incs} ${incs}) - target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search) - - PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "") - - PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_agast "${SUBSYS_NAME}" src/ni_agast.cpp) target_link_libraries(pcl_ni_agast pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_keypoints pcl_surface pcl_search) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_brisk "${SUBSYS_NAME}" src/ni_brisk.cpp) + target_link_libraries(pcl_ni_brisk pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_sample_consensus pcl_features pcl_keypoints pcl_surface pcl_search) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_susan "${SUBSYS_NAME}" src/ni_susan.cpp) target_link_libraries(pcl_ni_susan pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search) @@ -211,7 +199,7 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni_klt "${SUBSYS_NAME}" src/openni_klt.cpp) target_link_libraries(pcl_openni_klt pcl_common pcl_io pcl_visualization pcl_tracking) - endif() # OPENNI_FOUND + BUILD_OPENNI + endif() # WITH_OPENNI endif() # VTK_FOUND @@ -231,4 +219,23 @@ if(build) add_subdirectory("${CMAKE_CURRENT_SOURCE_DIR}/${subdir}") endforeach(subdir) + if(QHULL_FOUND) + set(incs + "include/pcl/${SUBSYS_NAME}/dominant_plane_segmentation.h" + "include/pcl/${SUBSYS_NAME}/timer.h" + ${incs} + ) + set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/dominant_plane_segmentation.hpp") + set(srcs "src/dominant_plane_segmentation.cpp" ${srcs}) + endif() + + set(LIB_NAME "pcl_${SUBSYS_NAME}") + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${impl_incs} ${incs}) + target_link_libraries("${LIB_NAME}" pcl_common pcl_io pcl_filters pcl_visualization pcl_segmentation pcl_surface pcl_features pcl_sample_consensus pcl_search) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "" "" "" "" "") + # Install include files + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) + endif(build) + diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h index 68abba17..bcf1a134 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h @@ -129,15 +129,14 @@ namespace pcl ColorHandler::ConstPtr color_handler_; GeometryHandler::ConstPtr geometry_handler_; - - //We keep actual local copies of these. Eigen::Vector4f origin_; Eigen::Quaternionf orientation_; + bool template_cloud_set_; + //Internal Storage of the templated type of this cloud int point_type_; - bool template_cloud_set_; bool checkIfFinite (); @@ -152,7 +151,6 @@ namespace pcl point_type_ = PointTypeFlags::NONE; } - }; template <> inline void diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/qt.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/qt.h index e6500781..0214cb08 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/qt.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/qt.h @@ -65,6 +65,11 @@ #include #include #include +#include +#include +#include +#include +#include #endif // CLOUD_COMPOSER_QT_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h index 49ada156..437f6478 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h @@ -182,6 +182,7 @@ namespace pcl getToolName () const { return "MergeCloudTool";} }; + } } diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/tool_factory.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/tool_factory.h index be66a32b..37e58a58 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/tool_factory.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/tool_factory.h @@ -91,7 +91,9 @@ namespace pcl } } +Q_DECLARE_METATYPE (pcl::cloud_composer::ToolFactory*); + Q_DECLARE_INTERFACE(pcl::cloud_composer::ToolFactory, "cloud_composer.ToolFactory/1.0") -#endif //TOOL_FACTORY_H_ \ No newline at end of file +#endif //TOOL_FACTORY_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/toolbox_model.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/toolbox_model.h index 342b4d0e..a1fee6a7 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/toolbox_model.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/toolbox_model.h @@ -118,7 +118,6 @@ namespace pcl } Q_DECLARE_METATYPE (pcl::cloud_composer::ToolBoxModel); -Q_DECLARE_METATYPE (pcl::cloud_composer::ToolFactory*); Q_DECLARE_METATYPE (QStandardItemModel*); #endif //TOOLBOX_MODEL_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h index 5c9e141f..0ba62633 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h @@ -65,6 +65,9 @@ namespace pcl { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: SplitItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -107,4 +110,4 @@ namespace pcl -#endif //EUCLIDEAN_CLUSTERING_H_ \ No newline at end of file +#endif //EUCLIDEAN_CLUSTERING_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h index 9656e869..8a6ec86a 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h @@ -67,6 +67,9 @@ namespace pcl { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: NewItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -110,4 +113,4 @@ namespace pcl -#endif //FPFH_ESTIMATION_H_ \ No newline at end of file +#endif //FPFH_ESTIMATION_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/supervoxels.hpp b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/supervoxels.hpp index 69859f52..e0b3c52f 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/supervoxels.hpp +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/impl/supervoxels.hpp @@ -119,4 +119,4 @@ pcl::cloud_composer::SupervoxelsTool::performTemplatedAction (QList = QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: NewItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -107,4 +110,4 @@ namespace pcl -#endif //NORMAL_ESTIMATION_H_ \ No newline at end of file +#endif //NORMAL_ESTIMATION_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h index 69a5d576..b73a634a 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h @@ -68,6 +68,9 @@ { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: SplitItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h index 24bcd80c..76deb942 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h @@ -65,6 +65,9 @@ namespace pcl { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: ModifyItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -107,4 +110,4 @@ namespace pcl - #endif //SANITIZE_CLOUD_H_ \ No newline at end of file + #endif //SANITIZE_CLOUD_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h index d6613396..f5f11f80 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h @@ -65,6 +65,9 @@ namespace pcl { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: ModifyItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -107,4 +110,4 @@ namespace pcl -#endif //STATISTICAL_OUTLIER_REMOVAL_H_ \ No newline at end of file +#endif //STATISTICAL_OUTLIER_REMOVAL_H_ diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h index 2740acea..ae4bcf29 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h @@ -70,6 +70,9 @@ { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: SplitItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h index 3a01b40a..46600306 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h @@ -65,6 +65,9 @@ namespace pcl { Q_OBJECT Q_INTERFACES (pcl::cloud_composer::ToolFactory) +#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0) + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif public: ModifyItemTool* createTool (PropertiesModel* parameter_model, QObject* parent = 0) @@ -107,4 +110,4 @@ namespace pcl -#endif //VOXEL_GRID_DOWNSAMPLE_H_ \ No newline at end of file +#endif //VOXEL_GRID_DOWNSAMPLE_H_ diff --git a/apps/cloud_composer/src/cloud_view.cpp b/apps/cloud_composer/src/cloud_view.cpp index 8e435451..98f159cd 100644 --- a/apps/cloud_composer/src/cloud_view.cpp +++ b/apps/cloud_composer/src/cloud_view.cpp @@ -109,7 +109,6 @@ pcl::cloud_composer::CloudView::rowsInserted (const QModelIndex& parent, int sta { QStandardItem* new_item = parent_item->child(row); CloudComposerItem* item = dynamic_cast (new_item); - item = dynamic_cast (new_item); if (item) item->paintView (vis_); diff --git a/apps/cloud_composer/src/cloud_viewer.cpp b/apps/cloud_composer/src/cloud_viewer.cpp index a1a9e4c6..295bf5fc 100644 --- a/apps/cloud_composer/src/cloud_viewer.cpp +++ b/apps/cloud_composer/src/cloud_viewer.cpp @@ -64,9 +64,8 @@ pcl::cloud_composer::CloudViewer::addNewProject (ProjectModel* new_model) } void -pcl::cloud_composer::CloudViewer::modelChanged (int index) +pcl::cloud_composer::CloudViewer::modelChanged (int) { - CloudView* view = static_cast (this->widget (index)); view = view; emit newModelSelected (getModel ()); } diff --git a/apps/cloud_composer/src/items/cloud_item.cpp b/apps/cloud_composer/src/items/cloud_item.cpp index 5f975000..fd87dc47 100644 --- a/apps/cloud_composer/src/items/cloud_item.cpp +++ b/apps/cloud_composer/src/items/cloud_item.cpp @@ -113,7 +113,6 @@ pcl::cloud_composer::CloudItem::setTemplateCloudFromBlob () { if (! template_cloud_set_ ) { - int num_fields = cloud_blob_ptr_->fields.size (); std::vector::iterator end = cloud_blob_ptr_->fields.end (); std::vector::iterator itr = cloud_blob_ptr_->fields.begin (); QStringList field_names; diff --git a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp index ad86a060..81efe2a0 100644 --- a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp +++ b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp @@ -90,8 +90,6 @@ pcl::cloud_composer::RectangularFrustumSelector::OnLeftButtonUp () selected_mapper->SetInputData (all_points); #endif selected_mapper->ScalarVisibilityOff (); - - vtkIdTypeArray* ids = vtkIdTypeArray::SafeDownCast (all_points->GetPointData ()->GetArray ("OriginalIds")); selected_actor->GetProperty ()->SetColor (0.0, 1.0, 0.0); //(R,G,B) selected_actor->GetProperty ()->SetPointSize (3); diff --git a/apps/cloud_composer/tools/euclidean_clustering.cpp b/apps/cloud_composer/tools/euclidean_clustering.cpp index 2f7dbcba..9583b1d2 100644 --- a/apps/cloud_composer/tools/euclidean_clustering.cpp +++ b/apps/cloud_composer/tools/euclidean_clustering.cpp @@ -6,7 +6,11 @@ #include #include -Q_EXPORT_PLUGIN2(cloud_composer_euclidean_clustering_tool, pcl::cloud_composer::EuclideanClusteringToolFactory) +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_euclidean_clustering_tool, pcl::cloud_composer::EuclideanClusteringToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::EuclideanClusteringTool::EuclideanClusteringTool (PropertiesModel* parameter_model, QObject* parent) : SplitItemTool (parameter_model, parent) diff --git a/apps/cloud_composer/tools/fpfh_estimation.cpp b/apps/cloud_composer/tools/fpfh_estimation.cpp index 9dccc616..e75349f5 100644 --- a/apps/cloud_composer/tools/fpfh_estimation.cpp +++ b/apps/cloud_composer/tools/fpfh_estimation.cpp @@ -8,9 +8,11 @@ #include - -Q_EXPORT_PLUGIN2(cloud_composer_fpfh_estimation_tool, pcl::cloud_composer::FPFHEstimationToolFactory) - +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_fpfh_estimation_tool, pcl::cloud_composer::FPFHEstimationToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::FPFHEstimationTool::FPFHEstimationTool (PropertiesModel* parameter_model, QObject* parent) : NewItemTool (parameter_model, parent) @@ -25,7 +27,7 @@ pcl::cloud_composer::FPFHEstimationTool::~FPFHEstimationTool () } QList -pcl::cloud_composer::FPFHEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) +pcl::cloud_composer::FPFHEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList output; const CloudComposerItem* input_item; diff --git a/apps/cloud_composer/tools/normal_estimation.cpp b/apps/cloud_composer/tools/normal_estimation.cpp index c0cf1d84..e0e7802d 100644 --- a/apps/cloud_composer/tools/normal_estimation.cpp +++ b/apps/cloud_composer/tools/normal_estimation.cpp @@ -4,9 +4,11 @@ #include #include - -Q_EXPORT_PLUGIN2(cloud_composer_normal_estimation_tool, pcl::cloud_composer::NormalEstimationToolFactory) - +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_normal_estimation_tool, pcl::cloud_composer::NormalEstimationToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::NormalEstimationTool::NormalEstimationTool (PropertiesModel* parameter_model, QObject* parent) : NewItemTool (parameter_model, parent) @@ -21,7 +23,7 @@ pcl::cloud_composer::NormalEstimationTool::~NormalEstimationTool () } QList -pcl::cloud_composer::NormalEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) +pcl::cloud_composer::NormalEstimationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList output; const CloudComposerItem* input_item; diff --git a/apps/cloud_composer/tools/organized_segmentation.cpp b/apps/cloud_composer/tools/organized_segmentation.cpp index 83bb536e..716bba12 100644 --- a/apps/cloud_composer/tools/organized_segmentation.cpp +++ b/apps/cloud_composer/tools/organized_segmentation.cpp @@ -9,7 +9,11 @@ #include -Q_EXPORT_PLUGIN2(cloud_composer_organized_segmentation_tool, pcl::cloud_composer::OrganizedSegmentationToolFactory) +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_organized_segmentation_tool, pcl::cloud_composer::OrganizedSegmentationToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::OrganizedSegmentationTool::OrganizedSegmentationTool (PropertiesModel* parameter_model, QObject* parent) : SplitItemTool (parameter_model, parent) @@ -60,4 +64,4 @@ pcl::cloud_composer::OrganizedSegmentationToolFactory::createToolParameterModel return parameter_model; -} \ No newline at end of file +} diff --git a/apps/cloud_composer/tools/sanitize_cloud.cpp b/apps/cloud_composer/tools/sanitize_cloud.cpp index edfde181..a7886cd3 100644 --- a/apps/cloud_composer/tools/sanitize_cloud.cpp +++ b/apps/cloud_composer/tools/sanitize_cloud.cpp @@ -3,9 +3,11 @@ #include - -Q_EXPORT_PLUGIN2(cloud_composer_sanitize_cloud_tool, pcl::cloud_composer::SanitizeCloudToolFactory) - +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_sanitize_cloud_tool, pcl::cloud_composer::SanitizeCloudToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::SanitizeCloudTool::SanitizeCloudTool (PropertiesModel* parameter_model, QObject* parent) : ModifyItemTool (parameter_model, parent) diff --git a/apps/cloud_composer/tools/statistical_outlier_removal.cpp b/apps/cloud_composer/tools/statistical_outlier_removal.cpp index 88939536..6563132a 100644 --- a/apps/cloud_composer/tools/statistical_outlier_removal.cpp +++ b/apps/cloud_composer/tools/statistical_outlier_removal.cpp @@ -4,9 +4,11 @@ #include #include - -Q_EXPORT_PLUGIN2(cloud_composer_statistical_outlier_removal_tool, pcl::cloud_composer::StatisticalOutlierRemovalToolFactory) - +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_statistical_outlier_removal_tool, pcl::cloud_composer::StatisticalOutlierRemovalToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::StatisticalOutlierRemovalTool::StatisticalOutlierRemovalTool (PropertiesModel* parameter_model, QObject* parent) : ModifyItemTool (parameter_model, parent) @@ -21,7 +23,7 @@ pcl::cloud_composer::StatisticalOutlierRemovalTool::~StatisticalOutlierRemovalTo } QList -pcl::cloud_composer::StatisticalOutlierRemovalTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) +pcl::cloud_composer::StatisticalOutlierRemovalTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { QList output; const CloudComposerItem* input_item; @@ -93,4 +95,4 @@ pcl::cloud_composer::StatisticalOutlierRemovalToolFactory::createToolParameterMo parameter_model->addProperty ("Std Dev Thresh", 1.0, Qt::ItemIsEditable | Qt::ItemIsEnabled); return parameter_model; -} \ No newline at end of file +} diff --git a/apps/cloud_composer/tools/supervoxels.cpp b/apps/cloud_composer/tools/supervoxels.cpp index c385fa87..92b0781a 100644 --- a/apps/cloud_composer/tools/supervoxels.cpp +++ b/apps/cloud_composer/tools/supervoxels.cpp @@ -7,7 +7,11 @@ #include -Q_EXPORT_PLUGIN2(cloud_composer_voxel_superpixels_tool, pcl::cloud_composer::SupervoxelsToolFactory) +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_voxel_superpixels_tool, pcl::cloud_composer::SupervoxelsToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::SupervoxelsTool::SupervoxelsTool (PropertiesModel* parameter_model, QObject* parent) : SplitItemTool (parameter_model, parent) @@ -58,4 +62,4 @@ pcl::cloud_composer::SupervoxelsToolFactory::createToolParameterModel (QObject* parameter_model->addProperty ("Spatial Weight", 0.4, Qt::ItemIsEditable | Qt::ItemIsEnabled); return parameter_model; -} \ No newline at end of file +} diff --git a/apps/cloud_composer/tools/voxel_grid_downsample.cpp b/apps/cloud_composer/tools/voxel_grid_downsample.cpp index 59ef0987..0e81f535 100644 --- a/apps/cloud_composer/tools/voxel_grid_downsample.cpp +++ b/apps/cloud_composer/tools/voxel_grid_downsample.cpp @@ -5,9 +5,11 @@ #include - -Q_EXPORT_PLUGIN2(cloud_composer_voxel_grid_downsample_tool, pcl::cloud_composer::VoxelGridDownsampleToolFactory) - +#if QT_VERSION < QT_VERSION_CHECK(5, 0, 0) + Q_EXPORT_PLUGIN2(cloud_composer_voxel_grid_downsample_tool, pcl::cloud_composer::VoxelGridDownsampleToolFactory) +#else + Q_PLUGIN_METADATA(IID "cloud_composer.ToolFactory/1.0") +#endif pcl::cloud_composer::VoxelGridDownsampleTool::VoxelGridDownsampleTool (PropertiesModel* parameter_model, QObject* parent) : ModifyItemTool (parameter_model, parent) diff --git a/apps/in_hand_scanner/CMakeLists.txt b/apps/in_hand_scanner/CMakeLists.txt index a67dd5e2..5f9e86a3 100644 --- a/apps/in_hand_scanner/CMakeLists.txt +++ b/apps/in_hand_scanner/CMakeLists.txt @@ -23,7 +23,7 @@ elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") endif() #OpenNI -if(NOT OPENNI_FOUND OR NOT BUILD_OPENNI) +if(NOT WITH_OPENNI) set(DEFAULT AUTO_OFF) set(REASON "OpenNI was not found or was disabled by the user.") elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") diff --git a/apps/in_hand_scanner/src/icp.cpp b/apps/in_hand_scanner/src/icp.cpp index fcbd45c4..6bbfad3b 100644 --- a/apps/in_hand_scanner/src/icp.cpp +++ b/apps/in_hand_scanner/src/icp.cpp @@ -382,7 +382,7 @@ pcl::ihs::ICP::selectModelPoints (const MeshConstPtr& mesh_model, for (Mesh::VertexDataCloud::const_iterator it=cloud.begin (); it!=cloud.end (); ++it) { // Don't consider points that are facing away from the camera. - if ((T_inv * it->getNormalVector4fMap ()).z () < 0.f) + if ((T_inv.lazyProduct (it->getNormalVector4fMap ())).z () < 0.f) { PointNormal pt; pt.getVector4fMap () = it->getVector4fMap (); diff --git a/apps/in_hand_scanner/src/visibility_confidence.cpp b/apps/in_hand_scanner/src/visibility_confidence.cpp index 0b83a1b3..90ebfd6a 100644 --- a/apps/in_hand_scanner/src/visibility_confidence.cpp +++ b/apps/in_hand_scanner/src/visibility_confidence.cpp @@ -137,7 +137,7 @@ pcl::ihs::addDirection (const Eigen::Vector4f& normal, // acos (angle) = dot (a, b) / (norm (a) * norm (b) // m_sphere_vertices are already normalized unsigned int index = 0; - (aligned_direction.transpose () * pcl::ihs::dome.getVertices ()).maxCoeff (&index); + aligned_direction.transpose ().lazyProduct (pcl::ihs::dome.getVertices ()).maxCoeff (&index); // Set the observed direction bit at 'index' // http://stackoverflow.com/questions/47981/how-do-you-set-clear-and-toggle-a-single-bit-in-c/47990#47990 diff --git a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp index 7863af4b..2a3fe5bc 100644 --- a/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp +++ b/apps/include/pcl/apps/impl/dominant_plane_segmentation.hpp @@ -33,9 +33,11 @@ * */ +#ifndef Q_MOC_RUN #pragma once #include #include +#endif #include #include diff --git a/apps/include/pcl/apps/manual_registration.h b/apps/include/pcl/apps/manual_registration.h index 81934288..99454c1c 100644 --- a/apps/include/pcl/apps/manual_registration.h +++ b/apps/include/pcl/apps/manual_registration.h @@ -42,7 +42,9 @@ #include // Boost +#ifndef Q_MOC_RUN #include +#endif // PCL #include diff --git a/apps/modeler/CMakeLists.txt b/apps/modeler/CMakeLists.txt index 7181cd46..6424d3f8 100644 --- a/apps/modeler/CMakeLists.txt +++ b/apps/modeler/CMakeLists.txt @@ -3,19 +3,34 @@ set(SUBSUBSYS_DESC "PCLModeler: PCL based reconstruction platform") set(SUBSUBSYS_DEPS common geometry io filters sample_consensus segmentation visualization kdtree features surface octree registration keypoints tracking search apps) set(REASON "") -# Find VTK and QVTK -if(VTK_FOUND AND VTK_USE_QVTK) +# Find VTK +if(NOT VTK_FOUND) + set(DEFAULT AUTO_OFF) + set(REASON "VTK was not found.") +else(NOT VTK_FOUND) set(DEFAULT TRUE) set(REASON) set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") include("${VTK_USE_FILE}") -elseif(NOT VTK_FOUND) +endif(NOT VTK_FOUND) + +# QT4 Found? +if(NOT QT4_FOUND) set(DEFAULT AUTO_OFF) - set(REASON "VTK was not found.") -elseif(NOT VTK_USE_QVTK) + set(REASON "Qt4 was not found.") +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif(NOT QT4_FOUND) + +# QVTK? +if(NOT VTK_USE_QVTK) set(DEFAULT AUTO_OFF) set(REASON "VTK was not built with Qt support.") -endif(VTK_FOUND AND VTK_USE_QVTK) +elseif(NOT ${DEFAULT} STREQUAL "AUTO_OFF") + set(DEFAULT TRUE) + set(REASON) +endif(NOT VTK_USE_QVTK) # Default to not building for now if (${DEFAULT} STREQUAL "TRUE") @@ -30,7 +45,7 @@ PCL_ADD_DOC("${SUBSUBSYS_NAME}") if(build) include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") - # Set Qt files and resources here + # Set Qt files and resources here set(uis main_window.ui) set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h" "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/scene_tree.h" diff --git a/apps/modeler/src/scene_tree.cpp b/apps/modeler/src/scene_tree.cpp index 5d1d08a7..7ab52eba 100755 --- a/apps/modeler/src/scene_tree.cpp +++ b/apps/modeler/src/scene_tree.cpp @@ -480,7 +480,7 @@ pcl::modeler::SceneTree::dropEvent(QDropEvent * event) std::set previous_parents; for (QList::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin(); selected_cloud_meshes_it != selected_cloud_meshes.end(); - selected_cloud_meshes_it ++) + ++ selected_cloud_meshes_it) { CloudMeshItem* cloud_mesh_item = *selected_cloud_meshes_it; RenderWindowItem* render_window_item = dynamic_cast(cloud_mesh_item->parent()); @@ -493,7 +493,7 @@ pcl::modeler::SceneTree::dropEvent(QDropEvent * event) std::vector cloud_mesh_items; for (QList::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin(); selected_cloud_meshes_it != selected_cloud_meshes.end(); - selected_cloud_meshes_it ++) + ++ selected_cloud_meshes_it) { CloudMeshItem* cloud_mesh_item = *selected_cloud_meshes_it; if (dynamic_cast(cloud_mesh_item->parent()) == NULL) @@ -515,7 +515,7 @@ pcl::modeler::SceneTree::dropEvent(QDropEvent * event) for (std::set::iterator previous_parents_it = previous_parents.begin(); previous_parents_it != previous_parents.end(); - previous_parents_it ++) + ++ previous_parents_it) { (*previous_parents_it)->getRenderWindow()->updateAxes(); (*previous_parents_it)->getRenderWindow()->render(); @@ -535,7 +535,7 @@ pcl::modeler::SceneTree::dropMimeData(QTreeWidgetItem * parent, int, const QMime for (QList::iterator selected_cloud_meshes_it = selected_cloud_meshes.begin(); selected_cloud_meshes_it != selected_cloud_meshes.end(); - selected_cloud_meshes_it ++) + ++ selected_cloud_meshes_it) { CloudMeshItem* cloud_mesh_item_copy = new CloudMeshItem(render_window_item, *(*selected_cloud_meshes_it)); render_window_item->addChild(cloud_mesh_item_copy); diff --git a/apps/optronic_viewer/CMakeLists.txt b/apps/optronic_viewer/CMakeLists.txt index 24f9b5db..5767aa7e 100644 --- a/apps/optronic_viewer/CMakeLists.txt +++ b/apps/optronic_viewer/CMakeLists.txt @@ -48,7 +48,7 @@ if(build) include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/include") - # Set Qt files and resources here + # Set Qt files and resources here # set(uis main_window.ui) set(moc_incs "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/main_window.h" "include/pcl/${SUBSYS_NAME}/${SUBSUBSYS_NAME}/filter_window.h" diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h index f3bd9094..6eea74fc 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloud.h @@ -348,7 +348,7 @@ class Cloud : public Statistics /// @param pts a vector used to store the points whose coordinates are /// transformed. void - getDisplaySpacePoints (std::vector& pts) const; + getDisplaySpacePoints (Point3DVector& pts) const; /// @brief Returns a const reference to the internal representation of this /// object. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/localTypes.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/localTypes.h index f9cd41e6..7d0812c5 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/localTypes.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/localTypes.h @@ -62,6 +62,9 @@ typedef pcl::PointXYZRGBA Point3D; /// The type used as internal representation of a cloud object. typedef pcl::PointCloud Cloud3D; +/// The type for the 3D point vector in the point cloud. +typedef pcl::PointCloud::VectorType Point3DVector; + /// The type for boost shared pointer pointing to a PCL cloud object. typedef Cloud3D::Ptr PclCloudPtr; diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h index 45a18877..2df7bd1c 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h @@ -45,6 +45,13 @@ #include #include +#include +#include +#include +#include +#include +#include +#include #include // Forward declaration to prevent circular inclusion diff --git a/apps/point_cloud_editor/src/cloud.cpp b/apps/point_cloud_editor/src/cloud.cpp index 9a41f6b7..e330eeaa 100644 --- a/apps/point_cloud_editor/src/cloud.cpp +++ b/apps/point_cloud_editor/src/cloud.cpp @@ -254,15 +254,8 @@ Cloud::drawWithHighlightColor () const void Cloud::draw (bool disable_highlight) const { - SelectionPtr selection_ptr; - try - { - selection_ptr = selection_wk_ptr_.lock(); - } - catch (boost::bad_weak_ptr) - { - selection_ptr.reset(); - } + SelectionPtr selection_ptr = selection_wk_ptr_.lock(); + glPushAttrib(GL_CURRENT_BIT | GL_POINT_BIT | GL_COLOR_BUFFER_BIT); { glPointSize(point_size_); @@ -425,7 +418,7 @@ Cloud::getDisplaySpacePoint (unsigned int index) const } void -Cloud::getDisplaySpacePoints (std::vector& pts) const +Cloud::getDisplaySpacePoints (Point3DVector& pts) const { for(unsigned int i = 0; i < cloud_.size(); ++i) pts.push_back(getDisplaySpacePoint(i)); diff --git a/apps/point_cloud_editor/src/select2DTool.cpp b/apps/point_cloud_editor/src/select2DTool.cpp index 655a4131..c1c8adb2 100644 --- a/apps/point_cloud_editor/src/select2DTool.cpp +++ b/apps/point_cloud_editor/src/select2DTool.cpp @@ -94,7 +94,7 @@ Select2DTool::end (int x, int y, BitMask modifiers, BitMask) GLfloat project[16]; glGetFloatv(GL_PROJECTION_MATRIX, project); - std::vector ptsvec; + Point3DVector ptsvec; cloud_ptr_->getDisplaySpacePoints(ptsvec); for(unsigned int i = 0; i < ptsvec.size(); ++i) { diff --git a/apps/src/face_detection/face_trainer.cpp b/apps/src/face_detection/face_trainer.cpp new file mode 100644 index 00000000..bf859700 --- /dev/null +++ b/apps/src/face_detection/face_trainer.cpp @@ -0,0 +1,39 @@ +/* + * face_trainer.cpp + * + * Created on: 22 Sep 2012 + * Author: Aitor Aldoma + */ + +#include "pcl/recognition/face_detection/rf_face_detector_trainer.h" +#include + +int main(int argc, char ** argv) +{ + int ntrees = 10; + std::string forest_fn = "forest.txt"; + int n_features = 1000; + std::string directory = ""; + int use_normals = 0; + int num_images = 3000; + + pcl::console::parse_argument (argc, argv, "-ntrees", ntrees); + pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn); + pcl::console::parse_argument (argc, argv, "-n_features", n_features); + pcl::console::parse_argument (argc, argv, "-directory", directory); + pcl::console::parse_argument (argc, argv, "-use_normals", use_normals); + pcl::console::parse_argument (argc, argv, "-num_images", num_images); + + pcl::RFFaceDetectorTrainer fdrf; + fdrf.setForestFilename (forest_fn); + fdrf.setDirectory (directory); + fdrf.setWSize (80); + fdrf.setNumTrees (ntrees); + fdrf.setNumFeatures (n_features); + fdrf.setUseNormals (static_cast (use_normals)); + fdrf.setNumTrainingImages (num_images); + + //TODO: do some checks here..., fn file exists, directory exists, etc... + fdrf.trainWithDataProvider (); +} + diff --git a/apps/src/face_detection/filesystem_face_detection.cpp b/apps/src/face_detection/filesystem_face_detection.cpp new file mode 100644 index 00000000..e7aaccb8 --- /dev/null +++ b/apps/src/face_detection/filesystem_face_detection.cpp @@ -0,0 +1,264 @@ +/* + * openni_face_detection.cpp + * + * Created on: 22 Sep 2012 + * Author: Aitor Aldoma + */ + +#include "pcl/recognition/face_detection/rf_face_detector_trainer.h" +#include +#include +#include +#include +namespace bf = boost::filesystem; + +#include "pcl/apps/face_detection/face_detection_apps_utils.h" + +bool SHOW_GT = false; +bool VIDEO = false; + +template +void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map, + bool show_votes, const std::string & filename) +{ + pcl::PointCloud::Ptr scene (new pcl::PointCloud); + pcl::copyPointCloud (*scene_vis, *scene); + + fdrf.setInputCloud (scene); + + if (heat_map) + { + pcl::PointCloud::Ptr intensity_cloud (new pcl::PointCloud); + fdrf.setFaceHeatMapCloud (intensity_cloud); + } + + fdrf.detectFaces (); + + typedef typename pcl::traits::fieldList::type FieldListM; + + float rgb_m; + bool exists_m; + pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists (scene_vis->points[0], "rgb", exists_m, rgb_m)); + + std::cout << "Color exists:" << static_cast (exists_m) << std::endl; + if (exists_m) + { + pcl::PointCloud::Ptr to_visualize (new pcl::PointCloud); + pcl::copyPointCloud (*scene_vis, *to_visualize); + + pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize); + vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud"); + } else + { + vis.addPointCloud (scene_vis, "scene_cloud"); + } + + if (heat_map) + { + pcl::PointCloud::Ptr intensity_cloud (new pcl::PointCloud); + fdrf.getFaceHeatMap (intensity_cloud); + + pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity"); + vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map"); + } + + if (show_votes) + { + //display votes_ + /*pcl::PointCloud::Ptr votes_cloud(new pcl::PointCloud()); + fdrf.getVotes(votes_cloud); + pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0); + vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud"); + vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); + vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud"); + vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/ + + pcl::PointCloud::Ptr votes_cloud (new pcl::PointCloud ()); + fdrf.getVotes2 (votes_cloud); + pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity"); + vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud"); + vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); + } + + vis.addCoordinateSystem (0.1, "global"); + + std::vector heads; + fdrf.getDetectedFaces (heads); + face_detection_apps_utils::displayHeads (heads, vis); + + if (SHOW_GT) + { + //check if there is ground truth data + std::string pose_file (filename); + boost::replace_all (pose_file, ".pcd", "_pose.txt"); + + Eigen::Matrix4f pose_mat; + pose_mat.setIdentity (4, 4); + bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat); + + if (result) + { + Eigen::Vector3f ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2); + Eigen::Vector3f trans_vector = Eigen::Vector3f (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3)); + std::cout << ea << std::endl; + std::cout << trans_vector << std::endl; + + pcl::PointXYZ center_point; + center_point.x = trans_vector[0]; + center_point.y = trans_vector[1]; + center_point.z = trans_vector[2]; + vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere"); + + pcl::ModelCoefficients cylinder_coeff; + cylinder_coeff.values.resize (7); // We need 7 values + cylinder_coeff.values[0] = center_point.x; + cylinder_coeff.values[1] = center_point.y; + cylinder_coeff.values[2] = center_point.z; + + cylinder_coeff.values[3] = ea[0]; + cylinder_coeff.values[4] = ea[1]; + cylinder_coeff.values[5] = ea[2]; + + Eigen::Vector3f vec = Eigen::Vector3f::UnitZ () * -1.f; + Eigen::Matrix3f matrixxx; + + matrixxx = Eigen::AngleAxisf (ea[0], Eigen::Vector3f::UnitX ()) * Eigen::AngleAxisf (ea[1], Eigen::Vector3f::UnitY ()) + * Eigen::AngleAxisf (ea[2], Eigen::Vector3f::UnitZ ()); + + //matrixxx = pose_mat.block<3,3>(0,0); + vec = matrixxx * vec; + + cylinder_coeff.values[3] = vec[0]; + cylinder_coeff.values[4] = vec[1]; + cylinder_coeff.values[5] = vec[2]; + + cylinder_coeff.values[6] = 0.01f; + vis.addCylinder (cylinder_coeff, "cylinder"); + } + } + + vis.setRepresentationToSurfaceForAllActors (); + + if (VIDEO) + { + vis.spinOnce (50, true); + } else + { + vis.spin (); + } + + vis.removeAllPointClouds (); + vis.removeAllShapes (); +} + +int main(int argc, char ** argv) +{ + int STRIDE_SW = 5; + std::string forest_fn = "forest.txt"; + int use_normals = 0; + float trans_max_variance = 800.f; + int min_votes_size = 400; + float face_threshold = 0.95f; + int heat_map = 1; + int show_votes = 0; + std::string test_directory; + std::string filename; + int rgb_exists = 0; + int pose_refinement_ = 0; + int icp_iterations = 5; + std::string model_path_; + + pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn); + pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance); + pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size); + pcl::console::parse_argument (argc, argv, "-use_normals", use_normals); + pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold); + pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW); + pcl::console::parse_argument (argc, argv, "-heat_map", heat_map); + pcl::console::parse_argument (argc, argv, "-show_votes", show_votes); + pcl::console::parse_argument (argc, argv, "-test_directory", test_directory); + pcl::console::parse_argument (argc, argv, "-filename", filename); + pcl::console::parse_argument (argc, argv, "-rgb_exists", rgb_exists); + pcl::console::parse_argument (argc, argv, "-show_gt", SHOW_GT); + pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_); + pcl::console::parse_argument (argc, argv, "-model_path", model_path_); + pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations); + pcl::console::parse_argument (argc, argv, "-video", VIDEO); + + pcl::RFFaceDetectorTrainer fdrf; + fdrf.setForestFilename (forest_fn); + fdrf.setWSize (80); + fdrf.setUseNormals (static_cast (use_normals)); + fdrf.setWStride (STRIDE_SW); + fdrf.setLeavesFaceMaxVariance (trans_max_variance); + fdrf.setLeavesFaceThreshold (face_threshold); + fdrf.setFaceMinVotes (min_votes_size); + + if (pose_refinement_) + { + fdrf.setPoseRefinement (true, icp_iterations); + fdrf.setModelPath (model_path_); + } + +//load forest from file and pass it to the detector + std::filebuf fb; + fb.open (forest_fn.c_str (), std::ios::in); + std::istream os (&fb); + + typedef pcl::face_detection::RFTreeNode NodeType; + pcl::DecisionForest forest; + forest.deserialize (os); + fb.close (); + + fdrf.setForest (forest); + + pcl::visualization::PCLVisualizer vis ("PCL Face detection"); + + if (test_directory.compare ("") != 0) + { + //recognize all files in directory... + std::string start = ""; + std::string ext = std::string ("pcd"); + bf::path dir = test_directory; + + std::vector files; + face_detection_apps_utils::getFilesInDirectory (dir, start, files, ext); + + std::sort (files.begin (), files.end (), face_detection_apps_utils::sortFiles); + + for (size_t i = 0; i < files.size (); i++) + { + std::stringstream file_to_process; + file_to_process << test_directory << "/" << files[i]; + std::string file = file_to_process.str (); + std::cout << file << std::endl; + + if (rgb_exists) + { + std::cout << "Loading a PointXYZRGBA cloud..." << std::endl; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::io::loadPCDFile (file, *cloud); + run (fdrf, cloud, vis, heat_map, show_votes, file); + } else + { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::io::loadPCDFile (file, *cloud); + run (fdrf, cloud, vis, heat_map, show_votes, file); + } + } + } else + { + if (rgb_exists) + { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::io::loadPCDFile (filename, *cloud); + run (fdrf, cloud, vis, heat_map, show_votes, filename); + } else + { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::io::loadPCDFile (filename, *cloud); + run (fdrf, cloud, vis, heat_map, show_votes, filename); + } + } +} + diff --git a/apps/src/face_detection/openni_face_detection.cpp b/apps/src/face_detection/openni_face_detection.cpp new file mode 100644 index 00000000..aea92209 --- /dev/null +++ b/apps/src/face_detection/openni_face_detection.cpp @@ -0,0 +1,140 @@ +/* + * openni_face_detection.cpp + * + * Created on: 22 Sep 2012 + * Author: Aitor Aldoma + */ + +#include "pcl/recognition/face_detection/rf_face_detector_trainer.h" +#include "pcl/apps/face_detection/openni_frame_source.h" +#include "pcl/apps/face_detection/face_detection_apps_utils.h" +#include +#include +#include + +void run(pcl::RFFaceDetectorTrainer & fdrf, bool heat_map = false, bool show_votes = false) +{ + OpenNIFrameSource::OpenNIFrameSource camera; + OpenNIFrameSource::PointCloudPtr scene_vis; + + pcl::visualization::PCLVisualizer vis ("Face dection"); + vis.addCoordinateSystem (0.1, "global"); + + //keyboard callback to stop getting frames and finalize application + boost::function keyboard_cb = boost::bind (&OpenNIFrameSource::OpenNIFrameSource::onKeyboardEvent, &camera, + _1); + vis.registerKeyboardCallback (keyboard_cb); + + while (camera.isActive ()) + { + scene_vis = camera.snap (); + + pcl::PointCloud::Ptr scene (new pcl::PointCloud ()); + pcl::copyPointCloud (*scene_vis, *scene); + + fdrf.setInputCloud (scene); + + if (heat_map) + { + pcl::PointCloud::Ptr intensity_cloud (new pcl::PointCloud); + fdrf.setFaceHeatMapCloud (intensity_cloud); + } + + { + pcl::ScopeTime t ("Detect faces..."); + fdrf.detectFaces (); + } + pcl::visualization::PointCloudColorHandlerRGBField handler_keypoints (scene_vis); + vis.addPointCloud < OpenNIFrameSource::PointT > (scene_vis, handler_keypoints, "scene_cloud"); + + if (heat_map) + { + pcl::PointCloud::Ptr intensity_cloud (new pcl::PointCloud); + fdrf.getFaceHeatMap (intensity_cloud); + pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity"); + vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map"); + } + + if (show_votes) + { + //display votes_ + pcl::PointCloud::Ptr votes_cloud (new pcl::PointCloud ()); + fdrf.getVotes (votes_cloud); + pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes (votes_cloud, 255, 0, 0); + vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud"); + vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud"); + vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud"); + vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud"); + } + + std::vector heads; + fdrf.getDetectedFaces (heads); + + face_detection_apps_utils::displayHeads (heads, vis); + + vis.setRepresentationToSurfaceForAllActors (); + vis.spinOnce (); + + vis.removeAllPointClouds (); + vis.removeAllShapes (); + } +} + +//./bin/pcl_openni_face_detector -face_threshold 0.99 -max_variance 2400 -min_votes_size 300 -stride_sw 4 -heat_map 0 -show_votes 1 -pose_refinement 1 -icp_iterations 5 -model_path face_model.pcd -forest_fn forest.txt +int main(int argc, char ** argv) +{ + int STRIDE_SW = 4; + int use_normals = 0; + float trans_max_variance = 1600.f; + int min_votes_size = 300; + float face_threshold = 0.99f; + int heat_map = 1; + int show_votes = 0; + int pose_refinement_ = 0; + int icp_iterations = 5; + + std::string forest_fn = "../data/forests/forest.txt"; + std::string model_path_ = "../data/ply_models/face.pcd"; + + pcl::console::parse_argument (argc, argv, "-forest_fn", forest_fn); + pcl::console::parse_argument (argc, argv, "-max_variance", trans_max_variance); + pcl::console::parse_argument (argc, argv, "-min_votes_size", min_votes_size); + pcl::console::parse_argument (argc, argv, "-use_normals", use_normals); + pcl::console::parse_argument (argc, argv, "-face_threshold", face_threshold); + pcl::console::parse_argument (argc, argv, "-stride_sw", STRIDE_SW); + pcl::console::parse_argument (argc, argv, "-heat_map", heat_map); + pcl::console::parse_argument (argc, argv, "-show_votes", show_votes); + pcl::console::parse_argument (argc, argv, "-pose_refinement", pose_refinement_); + pcl::console::parse_argument (argc, argv, "-model_path", model_path_); + pcl::console::parse_argument (argc, argv, "-icp_iterations", icp_iterations); + + pcl::RFFaceDetectorTrainer fdrf; + fdrf.setForestFilename (forest_fn); + fdrf.setWSize (80); + fdrf.setUseNormals (static_cast (use_normals)); + fdrf.setWStride (STRIDE_SW); + fdrf.setLeavesFaceMaxVariance (trans_max_variance); + fdrf.setLeavesFaceThreshold (face_threshold); + fdrf.setFaceMinVotes (min_votes_size); + + if (pose_refinement_) + { + fdrf.setPoseRefinement (true, icp_iterations); + fdrf.setModelPath (model_path_); + } + +//load forest from file and pass it to the detector + std::filebuf fb; + fb.open (forest_fn.c_str (), std::ios::in); + std::istream os (&fb); + + typedef pcl::face_detection::RFTreeNode NodeType; + pcl::DecisionForest forest; + forest.deserialize (os); + fb.close (); + + fdrf.setForest (forest); + + run (fdrf, heat_map, show_votes); +} + diff --git a/apps/src/face_detection/openni_frame_source.cpp b/apps/src/face_detection/openni_frame_source.cpp new file mode 100644 index 00000000..cd112b97 --- /dev/null +++ b/apps/src/face_detection/openni_frame_source.cpp @@ -0,0 +1,52 @@ +#include "pcl/apps/face_detection/openni_frame_source.h" +#include +#include +#include + +namespace OpenNIFrameSource +{ + + OpenNIFrameSource::OpenNIFrameSource(const std::string& device_id) : + grabber_ (device_id), most_recent_frame_ (), frame_counter_ (0), active_ (true) + { + boost::function frame_cb = boost::bind (&OpenNIFrameSource::onNewFrame, this, _1); + grabber_.registerCallback (frame_cb); + grabber_.start (); + } + + OpenNIFrameSource::~OpenNIFrameSource() + { + // Stop the grabber when shutting down + grabber_.stop (); + } + + bool OpenNIFrameSource::isActive() + { + return active_; + } + + const PointCloudPtr OpenNIFrameSource::snap() + { + return (most_recent_frame_); + } + + void OpenNIFrameSource::onNewFrame(const PointCloudConstPtr &cloud) + { + mutex_.lock (); + ++frame_counter_; + most_recent_frame_ = boost::make_shared < PointCloud > (*cloud); // Make a copy of the frame + mutex_.unlock (); + } + + void OpenNIFrameSource::onKeyboardEvent(const pcl::visualization::KeyboardEvent & event) + { + // When the spacebar is pressed, trigger a frame capture + mutex_.lock (); + if (event.keyDown () && event.getKeySym () == "e") + { + active_ = false; + } + mutex_.unlock (); + } + +} diff --git a/apps/src/ni_agast.cpp b/apps/src/ni_agast.cpp index a47ee7cc..d5772230 100644 --- a/apps/src/ni_agast.cpp +++ b/apps/src/ni_agast.cpp @@ -216,7 +216,7 @@ class AGASTDemo keypoints3d.height = keypoints->height; keypoints3d.is_dense = true; - int j = 0; + size_t j = 0; for (size_t i = 0; i < keypoints->size (); ++i) { const PointT &pt = (*cloud)(static_cast (keypoints->points[i].u), diff --git a/apps/src/ni_brisk.cpp b/apps/src/ni_brisk.cpp new file mode 100644 index 00000000..891a183d --- /dev/null +++ b/apps/src/ni_brisk.cpp @@ -0,0 +1,305 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: openni_viewer.cpp 5059 2012-03-14 02:12:17Z gedikli $ + * + */ + +#define SHOW_FPS 1 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl; +using namespace std; + +typedef PointXYZRGBA PointT; +typedef PointWithScale KeyPointT; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +class BRISKDemo +{ + public: + typedef PointCloud Cloud; + typedef Cloud::Ptr CloudPtr; + typedef Cloud::ConstPtr CloudConstPtr; + + BRISKDemo (Grabber& grabber) + : cloud_viewer_ ("BRISK 2D Keypoints -- PointCloud") + , grabber_ (grabber) + , image_viewer_ ("BRISK 2D Keypoints -- Image") + { + } + + ///////////////////////////////////////////////////////////////////////// + void + cloud_callback (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + + // Compute BRISK keypoints + BriskKeypoint2D brisk; + brisk.setThreshold (60); + brisk.setOctaves (4); + brisk.setInputCloud (cloud); + + keypoints_.reset (new PointCloud); + brisk.compute (*keypoints_); + } + + ///////////////////////////////////////////////////////////////////////// + void + init () + { + boost::function cloud_cb = boost::bind (&BRISKDemo::cloud_callback, this, _1); + cloud_connection = grabber_.registerCallback (cloud_cb); + } + + ///////////////////////////////////////////////////////////////////////// + string + getStrBool (bool state) + { + stringstream ss; + ss << state; + return (ss.str ()); + } + + ///////////////////////////////////////////////////////////////////////// + inline PointT + bilinearInterpolation (const CloudConstPtr &cloud, float x, float y) + { + int u = int (x); + int v = int (y); + + PointT pt; + pt.x = pt.y = pt.z = 0; + + const PointT &p1 = (*cloud)(u, v); + const PointT &p2 = (*cloud)(u+1, v); + const PointT &p3 = (*cloud)(u, v+1); + const PointT &p4 = (*cloud)(u+1, v+1); + + float fx = x - float (u), fy = y - float (v); + float fx1 = 1.0f - fx, fy1 = 1.0f - fy; + + float w1 = fx1 * fy1, w2 = fx * fy1, w3 = fx1 * fy, w4 = fx * fy; + float weight = 0; + + if (isFinite (p1)) + { + pt.x += p1.x * w1; + pt.y += p1.y * w1; + pt.z += p1.z * w1; + weight += w1; + } + if (isFinite (p2)) + { + pt.x += p2.x * w2; + pt.y += p2.y * w2; + pt.z += p2.z * w2; + weight += w2; + } + if (isFinite (p3)) + { + pt.x += p3.x * w3; + pt.y += p3.y * w3; + pt.z += p3.z * w3; + weight += w3; + } + if (isFinite (p4)) + { + pt.x += p4.x * w4; + pt.y += p4.y * w4; + pt.z += p4.z * w4; + weight += w4; + } + + if (weight == 0) + pt.x = pt.y = pt.z = std::numeric_limits::quiet_NaN (); + else + { + weight = 1.0f / weight; + pt.x *= weight; pt.y *= weight; pt.z *= weight; + } + + return (pt); + } + + ///////////////////////////////////////////////////////////////////////// + void + get3DKeypoints ( + const CloudConstPtr &cloud, + const PointCloud::Ptr &keypoints, PointCloud &keypoints3d) + { + keypoints3d.resize (keypoints->size ()); + keypoints3d.width = keypoints->width; + keypoints3d.height = keypoints->height; + keypoints3d.is_dense = true; + + size_t j = 0; + for (size_t i = 0; i < keypoints->size (); ++i) + { + PointT pt = bilinearInterpolation (cloud, keypoints->points[i].x, keypoints->points[i].y); + + keypoints3d.points[j].x = pt.x; + keypoints3d.points[j].y = pt.y; + keypoints3d.points[j].z = pt.z; + ++j; + } + + if (j != keypoints->size ()) + { + keypoints3d.resize (j); + keypoints3d.width = j; + keypoints3d.height = 1; + } + } + + ///////////////////////////////////////////////////////////////////////// + void + run () + { + grabber_.start (); + + bool image_init = false, cloud_init = false; + bool keypts = true; + + PointCloud::Ptr keypoints; + CloudConstPtr cloud; + CloudPtr keypoints3d (new Cloud); + + while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ()) + { + if (cloud_mutex_.try_lock ()) + { + if (cloud_) + cloud_.swap (cloud); + + if (keypoints_) + keypoints_.swap (keypoints); + + cloud_mutex_.unlock (); + + if (!cloud) + continue; + + if (!cloud_init) + { + cloud_viewer_.setPosition (0, 0); + cloud_viewer_.setSize (cloud->width, cloud->height); + cloud_init = !cloud_init; + } + + if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud")) + { + cloud_viewer_.addPointCloud (cloud, "OpenNICloud"); + cloud_viewer_.resetCameraViewpoint ("OpenNICloud"); + } + + if (!image_init) + { + image_viewer_.setPosition (cloud->width, 0); + image_viewer_.setSize (cloud->width, cloud->height); + image_init = !image_init; + } + + image_viewer_.showRGBImage (cloud); + + image_viewer_.removeLayer (getStrBool (keypts)); + for (size_t i = 0; i < keypoints->size (); ++i) + { + int u = int (keypoints->points[i].x); + int v = int (keypoints->points[i].y); + image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10, getStrBool (!keypts)); + } + keypts = !keypts; + + + get3DKeypoints (cloud, keypoints, *keypoints3d); + visualization::PointCloudColorHandlerCustom blue (keypoints3d, 0, 0, 255); + if (!cloud_viewer_.updatePointCloud (keypoints3d, blue, "keypoints")) + cloud_viewer_.addPointCloud (keypoints3d, blue, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints"); + } + + cloud_viewer_.spinOnce (); + image_viewer_.spinOnce (); + boost::this_thread::sleep (boost::posix_time::microseconds (100)); + } + + grabber_.stop (); + + cloud_connection.disconnect (); + } + + visualization::PCLVisualizer cloud_viewer_; + Grabber& grabber_; + boost::mutex cloud_mutex_; + CloudConstPtr cloud_; + + visualization::ImageViewer image_viewer_; + + PointCloud::Ptr keypoints_; + + private: + boost::signals2::connection cloud_connection; +}; + +/* ---[ */ +int +main (int, char**) +{ + string device_id ("#1"); + OpenNIGrabber grabber (device_id); + BRISKDemo openni_viewer (grabber); + + openni_viewer.init (); + openni_viewer.run (); + + return (0); +} +/* ]--- */ diff --git a/apps/src/openni_change_viewer.cpp b/apps/src/openni_change_viewer.cpp index a85025f1..4fda64ba 100644 --- a/apps/src/openni_change_viewer.cpp +++ b/apps/src/openni_change_viewer.cpp @@ -89,7 +89,7 @@ class OpenNIChangeViewer filtered_cloud.reset (new pcl::PointCloud (*cloud)); filtered_cloud->points.reserve(newPointIdxVector->size()); - for (std::vector::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++) + for (std::vector::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); ++it) filtered_cloud->points[*it].rgba = 255<<16; if (!viewer.wasStopped()) @@ -101,7 +101,7 @@ class OpenNIChangeViewer filtered_cloud->points.reserve(newPointIdxVector->size()); - for (std::vector::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); it++) + for (std::vector::iterator it = newPointIdxVector->begin (); it != newPointIdxVector->end (); ++it) filtered_cloud->points.push_back(cloud->points[*it]); diff --git a/apps/src/openni_organized_edge_detection.cpp b/apps/src/openni_organized_edge_detection.cpp new file mode 100644 index 00000000..d853b919 --- /dev/null +++ b/apps/src/openni_organized_edge_detection.cpp @@ -0,0 +1,303 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + + +#include +#include +#include +#include +#include +#include + +typedef pcl::PointXYZRGBA PointT; + +class OpenNIOrganizedEdgeDetection +{ + private: + boost::shared_ptr viewer; + pcl::PointCloud cloud_; + boost::mutex cloud_mutex; + + public: + OpenNIOrganizedEdgeDetection () + : viewer (new pcl::visualization::PCLVisualizer ("PCL Organized Edge Detection")) + { + + } + ~OpenNIOrganizedEdgeDetection () + { + } + + boost::shared_ptr + initCloudViewer (pcl::PointCloud::ConstPtr cloud) + { + viewer->setSize (640, 480); + viewer->addPointCloud (cloud, "cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + viewer->addCoordinateSystem (0.2f, "global"); + viewer->initCameraParameters (); + viewer->registerKeyboardCallback (&OpenNIOrganizedEdgeDetection::keyboard_callback, *this); + viewer->resetCameraViewpoint ("cloud"); + + const int point_size = 2; + viewer->addPointCloud (cloud, "nan boundary edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges"); + + viewer->addPointCloud (cloud, "occluding edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges"); + + viewer->addPointCloud (cloud, "occluded edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges"); + + viewer->addPointCloud (cloud, "high curvature edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges"); + + viewer->addPointCloud (cloud, "rgb edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges"); + + return (viewer); + } + + void + keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) + { + double opacity; + if (event.keyUp()) + { + switch (event.getKeyCode()) + { + case '1': + viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges"); + break; + case '2': + viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges"); + break; + case '3': + viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges"); + break; + case '4': + viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges"); + break; + case '5': + viewer->getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges"); + break; + } + } + } + + void + cloud_cb_ (const pcl::PointCloud::ConstPtr& cloud) + { + if (!viewer->wasStopped ()) + { + cloud_mutex.lock (); + cloud_ = *cloud; + cloud_mutex.unlock (); + } + } + + void + run () + { + pcl::Grabber* interface = new pcl::OpenNIGrabber (); + + boost::function::ConstPtr&)> f = boost::bind (&OpenNIOrganizedEdgeDetection::cloud_cb_, this, _1); + + // Make and initialize a cloud viewer + pcl::PointCloud::Ptr init_cloud_ptr (new pcl::PointCloud); + viewer = initCloudViewer (init_cloud_ptr); + boost::signals2::connection c = interface->registerCallback (f); + + interface->start (); + + pcl::IntegralImageNormalEstimation ne; + ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); + ne.setNormalSmoothingSize (10.0f); + ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR); + + float th_dd = 0.04f; + int max_search = 100; + pcl::OrganizedEdgeFromRGBNormals oed; + oed.setDepthDisconThreshold (th_dd); + oed.setMaxSearchNeighbors (max_search); + + oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED); + //oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY); + + pcl::PointCloud labels; + std::vector label_indices; + + while (!viewer->wasStopped ()) + { + viewer->spinOnce (); + + if (cloud_mutex.try_lock ()) + { + labels.clear (); + label_indices.clear (); + + double normal_start = pcl::getTime (); + + if (oed.getEdgeType () & oed.EDGELABEL_HIGH_CURVATURE) + { + pcl::PointCloud::Ptr normal_cloud (new pcl::PointCloud); + ne.setInputCloud (cloud_.makeShared ()); + ne.compute (*normal_cloud); + double normal_end = pcl::getTime (); + std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl; + + oed.setInputNormals (normal_cloud); + } + + double oed_start = pcl::getTime (); + + oed.setInputCloud (cloud_.makeShared ()); + oed.compute (labels, label_indices); + + double oed_end = pcl::getTime (); + std::cout << "Edge Detection took " << double (oed_end - oed_start) << std::endl; + std::cout << "Frame took " << double (oed_end - normal_start) << std::endl; + + // Make gray point cloud + for (size_t idx = 0; idx < cloud_.points.size (); idx++) + { + pcl::uint8_t gray = pcl::uint8_t((cloud_.points[idx].r + cloud_.points[idx].g + cloud_.points[idx].b)/3); + cloud_.points[idx].r = cloud_.points[idx].g = cloud_.points[idx].b = gray; + } + + // Show the gray point cloud + if (!viewer->updatePointCloud (cloud_.makeShared (), "cloud")) + viewer->addPointCloud (cloud_.makeShared (), "cloud"); + + // Show edges + pcl::PointCloud::Ptr occluding_edges (new pcl::PointCloud), + occluded_edges (new pcl::PointCloud), + nan_boundary_edges (new pcl::PointCloud), + high_curvature_edges (new pcl::PointCloud), + rgb_edges (new pcl::PointCloud); + + pcl::copyPointCloud (cloud_, label_indices[0].indices, *nan_boundary_edges); + pcl::copyPointCloud (cloud_, label_indices[1].indices, *occluding_edges); + pcl::copyPointCloud (cloud_, label_indices[2].indices, *occluded_edges); + pcl::copyPointCloud (cloud_, label_indices[3].indices, *high_curvature_edges); + pcl::copyPointCloud (cloud_, label_indices[4].indices, *rgb_edges); + + if (!viewer->updatePointCloud (nan_boundary_edges, "nan boundary edges")) + viewer->addPointCloud (nan_boundary_edges, "nan boundary edges"); + + if (!viewer->updatePointCloud (occluding_edges, "occluding edges")) + viewer->addPointCloud (occluding_edges, "occluding edges"); + + if (!viewer->updatePointCloud (occluded_edges, "occluded edges")) + viewer->addPointCloud (occluded_edges, "occluded edges"); + + if (!viewer->updatePointCloud (high_curvature_edges, "high curvature edges")) + viewer->addPointCloud (high_curvature_edges, "high curvature edges"); + + if (!viewer->updatePointCloud (rgb_edges, "rgb edges")) + viewer->addPointCloud (rgb_edges, "rgb edges"); + + cloud_mutex.unlock (); + } + } + + interface->stop (); + } +}; + +void +usage (char ** argv) +{ + std::cout << "usage: " << argv[0] << " \n\n"; + + openni_wrapper::OpenNIDriver& driver = openni_wrapper::OpenNIDriver::getInstance (); + if (driver.getNumberDevices () > 0) + { + for (unsigned deviceIdx = 0; deviceIdx < driver.getNumberDevices (); ++deviceIdx) + { + cout << "Device: " << deviceIdx + 1 << ", vendor: " << driver.getVendorName (deviceIdx) << ", product: " << driver.getProductName (deviceIdx) + << ", connected: " << driver.getBus (deviceIdx) << " @ " << driver.getAddress (deviceIdx) << ", serial number: \'" << driver.getSerialNumber (deviceIdx) << "\'" << endl; + cout << "device_id may be #1, #2, ... for the first second etc device in the list or" << endl + << " bus@address for the device connected to a specific usb-bus / address combination (works only in Linux) or" << endl + << " (only in Linux and for devices which provide serial numbers)" << endl; + } + } + else + cout << "No devices connected." << endl; +} + +int +main (int argc, char ** argv) +{ + std::string arg; + if (argc > 1) + arg = std::string (argv[1]); + + if (arg == "--help" || arg == "-h") + { + usage (argv); + return 1; + } + + std::cout << "Press following keys to enable/disable the different edge types:" << std::endl; + std::cout << "<1> EDGELABEL_NAN_BOUNDARY edge" << std::endl; + std::cout << "<2> EDGELABEL_OCCLUDING edge" << std::endl; + std::cout << "<3> EDGELABEL_OCCLUDED edge" << std::endl; + //std::cout << "<4> EDGELABEL_HIGH_CURVATURE edge" << std::endl; + //std::cout << "<5> EDGELABEL_RGB_CANNY edge" << std::endl; + std::cout << " quit" << std::endl; + + + pcl::OpenNIGrabber grabber (""); + if (grabber.providesCallback ()) + { + OpenNIOrganizedEdgeDetection app; + app.run (); + } + else + PCL_ERROR ("The input device does not provide a PointXYZRGBA mode.\n"); + + return (0); +} diff --git a/apps/src/openni_tracking.cpp b/apps/src/openni_tracking.cpp index 4fd0d138..75f9fe95 100644 --- a/apps/src/openni_tracking.cpp +++ b/apps/src/openni_tracking.cpp @@ -449,7 +449,7 @@ public: } void addNormalToCloud (const CloudConstPtr &cloud, - const pcl::PointCloud::ConstPtr &normals, + const pcl::PointCloud::ConstPtr &, RefCloud &result) { result.width = cloud->width; diff --git a/apps/src/openni_uniform_sampling.cpp b/apps/src/openni_uniform_sampling.cpp index bfd544f8..06ac08f7 100644 --- a/apps/src/openni_uniform_sampling.cpp +++ b/apps/src/openni_uniform_sampling.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -80,14 +80,14 @@ class OpenNIUniformSampling FPS_CALC ("computation"); cloud_.reset (new Cloud); - indices_.reset (new pcl::PointCloud); keypoints_.reset (new pcl::PointCloud); // Computation goes here pass_.setInputCloud (cloud); - pass_.compute (*indices_); + pcl::PointCloud sampled; + pass_.filter (sampled); *cloud_ = *cloud; - pcl::copyPointCloud (*cloud, indices_->points, *keypoints_); + pcl::copyPointCloud (sampled, *keypoints_); } void @@ -138,7 +138,6 @@ class OpenNIUniformSampling boost::mutex mtx_; CloudPtr cloud_; pcl::PointCloud::Ptr keypoints_; - pcl::PointCloud::Ptr indices_; }; void diff --git a/apps/src/pcd_organized_edge_detection.cpp b/apps/src/pcd_organized_edge_detection.cpp new file mode 100644 index 00000000..1a176ed7 --- /dev/null +++ b/apps/src/pcd_organized_edge_detection.cpp @@ -0,0 +1,269 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl; +using namespace pcl::io; +using namespace pcl::console; + +float default_th_dd = 0.02f; +int default_max_search = 50; + +typedef pcl::PointCloud Cloud; +typedef Cloud::Ptr CloudPtr; +typedef Cloud::ConstPtr CloudConstPtr; + +pcl::visualization::PCLVisualizer viewer ("3D Edge Viewer"); + +void +printHelp (int, char **argv) +{ + print_error ("Syntax is: %s input.pcd output.pcd \n", argv[0]); + print_info (" where options are:\n"); + print_info (" -th_dd X = the tolerance in meters for difference in depth values between neighboring points (The value is set for 1 meter and is adapted with respect to depth value linearly. (e.g. 2.0*th_dd in 2 meter depth)) (default: "); + print_value ("%f", default_th_dd); print_info (")\n"); + print_info (" -max_search X = the max search distance for deciding occluding and occluded edges (default: "); + print_value ("%d", default_max_search); print_info (")\n"); +} + +bool +loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud) +{ + TicToc tt; + print_highlight ("Loading "); print_value ("%s ", filename.c_str ()); + + tt.tic (); + if (loadPCDFile (filename, cloud) < 0) + return (false); + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n"); + print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ()); + + return (true); +} + +void +saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output) +{ + TicToc tt; + tt.tic (); + + print_highlight ("Saving "); print_value ("%s ", filename.c_str ()); + + pcl::io::savePCDFile (filename, output, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); // Save as binary + + print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n"); +} + +void +keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) +{ + double opacity; + if (event.keyUp()) + { + switch (event.getKeyCode()) + { + case '1': + viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges"); + break; + case '2': + viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges"); + break; + case '3': + viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges"); + break; + case '4': + viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges"); + break; + case '5': + viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges"); + viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges"); + break; + } + } +} + +void +compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output, + float th_dd, int max_search) +{ + CloudPtr cloud (new Cloud); + fromPCLPointCloud2 (*input, *cloud); + + pcl::PointCloud::Ptr normal (new pcl::PointCloud); + pcl::IntegralImageNormalEstimation ne; + ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); + ne.setNormalSmoothingSize (10.0f); + ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR); + ne.setInputCloud (cloud); + ne.compute (*normal); + + TicToc tt; + tt.tic (); + + //OrganizedEdgeBase oed; + //OrganizedEdgeFromRGB oed; + //OrganizedEdgeFromNormals oed; + OrganizedEdgeFromRGBNormals oed; + oed.setInputNormals (normal); + oed.setInputCloud (cloud); + oed.setDepthDisconThreshold (th_dd); + oed.setMaxSearchNeighbors (max_search); + oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY); + PointCloud